diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index f3d91cd..26f8163 100644
--- a/launch/fulltest.launch
+++ b/launch/fulltest.launch
@@ -10,8 +10,6 @@
output="screen"/>
-
diff --git a/script/controller.py b/script/controller.py
index b8c9e73..f827985 100755
--- a/script/controller.py
+++ b/script/controller.py
@@ -1,10 +1,44 @@
#! /usr/bin/env python
import rospy
+import actionlib
from teleoperation.srv import InformController, InformControllerResponse
+from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
-STATE = 'idle' # Also walk, imitate, fallen, recognizing
+STATE = 'dead' # Also walk, imitate, fallen, idle
+speech_in_progress = False
+
+IMITATE = 'arms'
+KILL = 'kill'
+REVIVE = 'go'
+STOP = 'stop'
+
+voc_state = {
+ 'idle': [IMITATE, KILL],
+ 'imitate': [STOP, KILL],
+ 'dead': [REVIVE]
+}
+
+
+def speech_done_cb(_, result):
+ global speech_in_progress, STATE
+ _state_old = STATE
+ rospy.loginfo('SPEECH: {}'.format(result))
+ if not result:
+ speech_in_progress = False
+ return
+ if result.word == IMITATE and STATE == 'idle':
+ STATE = 'imitate'
+ elif result.word == STOP and STATE == 'imitate':
+ STATE = 'idle'
+ elif result.word == KILL and STATE in ('idle', 'imitate'):
+ STATE = 'dead'
+ elif result.word == REVIVE and STATE == 'dead':
+ STATE = 'idle'
+ if _state_old != STATE:
+ rospy.loginfo('{} -> {}'.format(_state_old, STATE))
+ speech_in_progress = False
def inform_controller_factory(who):
@@ -49,26 +83,6 @@ def handle_request(r):
if STATE == 'imitate':
permission = True
- elif module == 'speech':
- if message == 'recognize':
- if STATE in ('idle', 'imitate', 'dead'):
- permission = True
- elif message == 'imitate':
- if STATE == 'idle':
- STATE = 'imitate'
- permission = True
- elif message == 'stop':
- if STATE == 'imitate':
- STATE = 'idle'
- permission = True
- elif message == 'kill':
- STATE = 'dead'
- permission = True
- elif message == 'revive':
- if STATE == 'dead':
- STATE = 'idle'
- permission = True
-
rospy.logdebug(
'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % (
module, message, permission, STATE
@@ -80,6 +94,25 @@ def handle_request(r):
if __name__ == '__main__':
+
rospy.init_node('controller', log_level=rospy.INFO)
ic = rospy.Service('inform_controller', InformController, handle_request)
- rospy.spin()
+
+ client = actionlib.SimpleActionClient('speech_server',
+ RequestSpeechAction)
+ client.wait_for_server()
+ rospy.loginfo('SPEECH: SERVER THERE')
+
+ rospy.on_shutdown(lambda: speech_in_progress and client.cancel_goal())
+
+ while not rospy.is_shutdown():
+ if not STATE in ('idle', 'imitate', 'dead'):
+ if speech_in_progress:
+ client.cancel_goal()
+ speech_in_progress = False
+ else:
+ if not speech_in_progress:
+ speech_in_progress = True
+ client.send_goal(RequestSpeechGoal(voc_state[STATE]),
+ speech_done_cb)
+ rospy.Rate(10).sleep()
diff --git a/script/imitator.py b/script/imitator.py
index f31c37a..59f5677 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -1,17 +1,27 @@
#! /usr/bin/env python
import os
+import json
from time import sleep
from math import atan, asin, radians, sqrt
import rospy
import tf
+import numpy as np
from naoqi import ALProxy
from controller import inform_controller_factory
_inform_controller = inform_controller_factory('imitator')
+
+
_FRAME_TORSO = 0
+arm = None
+
+
+def _elbow(arm_):
+ quot = min(1.0, arm_ / arm)
+ return radians(180 * (1 - quot))
if __name__ == '__main__':
@@ -25,6 +35,12 @@ if __name__ == '__main__':
mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
[radians(-70), radians(70), -radians(40), radians(40)], 0.5)
+ here = os.path.dirname(os.path.realpath(__file__))
+ with open('{}/../config/default.yaml'.format(here)) as f:
+ x = json.load(f)
+
+ arm = x['arm']
+
while not rospy.is_shutdown():
sleep(0.1)
if not _inform_controller('imitate'):
@@ -63,12 +79,16 @@ if __name__ == '__main__':
roll -= sign * radians(25)
pitch = atan(-trans[2] / abs(trans[0]))
+ eroll = -sign * _elbow(np.linalg.norm(np.array(trans)))
+
mp.setAngles([
'{}ShoulderRoll'.format(eff),
- '{}ShoulderPitch'.format(eff)
+ '{}ShoulderPitch'.format(eff),
+ '{}ElbowRoll'.format(eff)
], [
roll,
- pitch
+ pitch,
+ eroll
], 0.3)
# targ = np.array(trans)
diff --git a/script/speech_client.py b/script/speech_client.py
deleted file mode 100755
index f4e9dba..0000000
--- a/script/speech_client.py
+++ /dev/null
@@ -1,68 +0,0 @@
-#! /usr/bin/env python
-# import os
-# from time import sleep
-
-import rospy
-import actionlib
-# from naoqi import ALProxy
-
-from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
-from controller import inform_controller_factory
-
-
-in_progress = False
-state = 'idle'
-
-IMITATE = 'repeat'
-KILL = 'kill'
-REVIVE = 'go'
-STOP = 'stop'
-
-voc_state = {
- 'idle': [IMITATE, KILL],
- 'imitate': [STOP, KILL],
- 'killed': [REVIVE]
-}
-
-_inform_controller = inform_controller_factory('speech')
-
-
-def done_cb(_, result):
- global in_progress, state
- rospy.loginfo('SPEECH CLIENT: {}'.format(result))
- if result is None:
- in_progress = False
- return
- if result.word == IMITATE and _inform_controller('imitate'):
- state = 'imitate'
- elif result.word == STOP and _inform_controller('stop'):
- state = 'idle'
- elif result.word == KILL and _inform_controller('kill'):
- state = 'killed'
- elif result.word == REVIVE and _inform_controller('revive'):
- state = 'idle'
- in_progress = False
-
-
-if __name__ == '__main__':
- rospy.init_node('speech_client')
- client = actionlib.SimpleActionClient('speech_server',
- RequestSpeechAction)
- client.wait_for_server()
- rospy.loginfo('SPEECH CLIENT: SERVER THERE')
-
- rospy.on_shutdown(lambda: client.cancel_goal())
-
- while not rospy.is_shutdown():
- if not _inform_controller('recognize'):
- if in_progress:
- client.cancel_goal()
- in_progress = False
- state = 'idle'
-
- else:
- if not in_progress:
- in_progress = True
- client.send_goal(RequestSpeechGoal(voc_state[state]),
- done_cb)
- rospy.Rate(4).sleep()
diff --git a/script/speech_server.py b/script/speech_server.py
index 1cc7e23..3302fe1 100755
--- a/script/speech_server.py
+++ b/script/speech_server.py
@@ -87,7 +87,7 @@ class SpeechDetectorModule(ALModule):
self.stop_speech()
else:
self.stop_speech(pause=True)
- self.tts.say('I didn\'t understand. Please repeat')
+ self.tts.say('nope')
self.start_speech(self.voc, resume=True)