did hands

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}
 )
 

launch/fulltest.launch

@@ -10,6 +10,7 @@
         output="screen"/>
     <node name="controller" pkg="teleoperation" type="controller.py"
         output="screen"/>
+    <node name="hand_ler" pkg="teleoperation" type="hand_ler.py"/>
     <node name="imitator" pkg="teleoperation" type="imitator.py"
         output="screen"/>
     <node name="walker" pkg="teleoperation" type="walker.py"/>

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()

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()

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

srv/Hands.srv

@@ -0,0 +1,3 @@
+string target
+-----
+string newstate