diff --git a/CMakeLists.txt b/CMakeLists.txt index 8768f02..e31f40c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ add_service_files( DIRECTORY srv FILES InformController.srv + Hands.srv ) add_action_files( @@ -39,10 +40,13 @@ include_directories( ) catkin_install_python(PROGRAMS + ./script/calibrator.py ./script/controller.py ./script/walker.py ./script/fall_detector.py ./script/imitator.py + ./script/hand_ler.py + ./script/speech_server.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/launch/fulltest.launch b/launch/fulltest.launch index 26f8163..86d2931 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -10,6 +10,7 @@ output="screen"/> + diff --git a/script/controller.py b/script/controller.py index 4b43610..ecd1974 100755 --- a/script/controller.py +++ b/script/controller.py @@ -5,6 +5,7 @@ import rospy import actionlib from teleoperation.srv import InformController, InformControllerResponse +from teleoperation.srv import Hands from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal @@ -22,10 +23,7 @@ VOC_HAND = { 'closed': [OPEN], 'opened': [CLOSE] } -SPEECH_HAND = { - OPEN: (('closed',), 'opened'), - CLOSE: (('opened',), 'closed') -} +SPEECH_HAND = (OPEN, CLOSE) AI = False # autoimitate @@ -53,6 +51,15 @@ def init_voc_state_speech(): }) +def hands(what): + try: + _hands = rospy.ServiceProxy('hands', Hands) + newstate = _hands(what).newstate + return newstate if newstate in ('opened', 'closed') else hand_state + except rospy.service.ServiceException: + return hand_state + + def speech_done_cb(_, result): global speech_in_progress, state, hand_state _state_old = state @@ -66,10 +73,7 @@ def speech_done_cb(_, result): if state in allowed: state = target elif result.word in SPEECH_HAND: - allowed, target = SPEECH_HAND[result.word] - if hand_state in allowed: - hand_state = target - + hand_state = hands(result.word) if _state_old != state: rospy.loginfo('{} -> {}'.format(_state_old, state)) speech_in_progress = False @@ -142,23 +146,24 @@ if __name__ == '__main__': ic = rospy.Service('inform_controller', InformController, handle_request) - client = actionlib.SimpleActionClient('speech_server', + speech = actionlib.SimpleActionClient('speech_server', RequestSpeechAction) - client.wait_for_server() + + speech.wait_for_server() rospy.loginfo('SPEECH: SERVER THERE') - rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal()) + rospy.on_shutdown(lambda: speech_in_progress and speech.cancel_goal()) while not rospy.is_shutdown(): if state in ('idle', 'imitate', 'dead'): if not speech_in_progress: speech_in_progress = True - client.send_goal(RequestSpeechGoal( + speech.send_goal(RequestSpeechGoal( VOC_STATE[state] + VOC_HAND[hand_state] ), speech_done_cb) else: if speech_in_progress: - client.cancel_goal() + speech.cancel_goal() speech_in_progress = False rospy.Rate(10).sleep() diff --git a/script/hand_ler.py b/script/hand_ler.py new file mode 100755 index 0000000..6693d9a --- /dev/null +++ b/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() diff --git a/script/speech_server.py b/script/speech_server.py index 3302fe1..02c922d 100755 --- a/script/speech_server.py +++ b/script/speech_server.py @@ -13,6 +13,7 @@ almem = None def request_speech(goal): + rospy.loginfo('SPEECH SERVER: NEW GOAL: {}'.format(goal)) if not speech_detector.start_speech(goal.vocabulary): sas.set_succeeded(RequestSpeechResult(word='')) return diff --git a/srv/Hands.srv b/srv/Hands.srv new file mode 100644 index 0000000..d73e378 --- /dev/null +++ b/srv/Hands.srv @@ -0,0 +1,3 @@ +string target +----- +string newstate