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