pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

did some work

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-31 17:44:15 +01:00
parent e368cd9abe
commit 3217b7f841
10 changed files with 323 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
script/calibrator.py
View File

@@ -1,11 +1,172 @@
#! /usr/bin/env python
import os
import json
from sys import argv
import rospy
import tf
import actionlib
import numpy as np
from naoqi import ALProxy
from controller import inform_controller_factory
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
_inform_controller = inform_controller_factory('calibrator')
def calibration():
tts.say('Stand in front of camera')
tts.say('Far enough to see your arms')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
trans, q = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
rot = tf.transformations.euler_from_quaternion(q)
center = trans, rot
except Exception:
pass
tts.say('Now stretch your arms')
tts.say('Make sure the markers are detected')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
larm, _ = ll.lookupTransform('Aruco_0_frame',
'Aruco_1_frame',
rospy.Time(0))
rarm, _ = ll.lookupTransform('Aruco_0_frame',
'Aruco_2_frame',
rospy.Time(0))
except Exception:
pass
tts.say('Now rotate to your right')
tts.say('So that marker is still detected')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
_, q = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
RR = tf.transformations.euler_from_quaternion(q)
except Exception:
pass
tts.say('Now rotate to your left')
tts.say('So that marker is still detected')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
_, q = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
LR = tf.transformations.euler_from_quaternion(q)
except Exception:
pass
tts.say('Now rotate to initial position')
tts.say('Take a big step forward')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
FW, _ = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
except Exception:
pass
tts.say('Now return to the initial position')
tts.say('Then take a big step backward')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
BK, _ = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
except Exception:
pass
tts.say('Now return to the initial position')
tts.say('Then take a big step to the right')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
RT, _ = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
except Exception:
pass
tts.say('Now return to the initial position')
tts.say('Then take a big step to the left')
tts.say('Then say start')
client.send_goal(RequestSpeechGoal(['start']))
client.wait_for_result()
try:
LT, _ = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
except Exception:
pass
calib = {}
try:
tts.say('Well done')
calib['cr'] = [center[0][0], center[0][1], center[1][2]]
calib['arm'] = (np.linalg.norm(np.array(larm)) +
np.linalg.norm(np.array(rarm))) / 2
calib['rr'] = RR[2]
calib['lr'] = LR[2]
calib['fw'] = FW[0]
calib['bk'] = BK[0]
calib['lt'] = LT[1]
calib['rt'] = RT[1]
here = os.path.dirname(os.path.realpath(__file__))
with open('{}/../config/default.yaml'.format(here), 'w') as f:
json.dump(calib, f)
except:
tts.say('Something has gone very wrong')
if __name__ == '__main__':
rospy.init_node('teleoperation/calibrator')
if len(argv) < 2:
raise SystemExit
rospy.init_node('calibrator', disable_signals=True)
try:
tts = ALProxy('ALTextToSpeech', os.environ['NAO_IP'], 9559)
client = actionlib.SimpleActionClient('speech_server',
RequestSpeechAction)
client.wait_for_server()
ll = tf.TransformListener()
rospy.on_shutdown(lambda: client.cancel_goal())
calibration()
except KeyboardInterrupt:
rospy.signal_shutdown('Interrupted')

6
script/controller.py
View File

@@ -4,7 +4,7 @@ import rospy
from teleoperation.srv import InformController, InformControllerResponse
STATE = 'idle' # Also walk, imitate and fallen
STATE = 'idle' # Also walk, imitate, fallen, recognizing
def inform_controller_factory(who):
@@ -49,7 +49,7 @@ def handle_request(r):
if STATE == 'imitate':
permission = True
elif module == 'speech_recognition':
elif module == 'speech':
if message == 'recognize':
if STATE in ('idle', 'imitate'):
permission = True
@@ -73,6 +73,6 @@ def handle_request(r):
if __name__ == '__main__':
rospy.init_node('teleoperation/controller', log_level=rospy.INFO)
rospy.init_node('controller', log_level=rospy.INFO)
ic = rospy.Service('inform_controller', InformController, handle_request)
rospy.spin()

18
script/imitator.py
View File

@@ -33,15 +33,15 @@ if __name__ == '__main__':
# torso movement
try:
_, q = ll.lookupTransform('odom',
'Aruco_0_frame',
rospy.Time(0))
rot = tf.transformations.euler_from_quaternion(q)
mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
[-rot[1], -rot[1]], 0.3)
except Exception as e:
rospy.logwarn(e)
# try:
# _, q = ll.lookupTransform('odom',
# 'Aruco_0_frame',
# rospy.Time(0))
# rot = tf.transformations.euler_from_quaternion(q)
# mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
# [-rot[1], -rot[1]], 0.3)
# except Exception as e:
# rospy.logwarn(e)
# continue

56
script/speech_client.py Executable file
View File

@@ -0,0 +1,56 @@
#! /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'
voc_state = {
'idle': 'start',
'imitate': 'stop'
}
_inform_controller = inform_controller_factory('speech')
def done_cb(_, result):
global in_progress, state
rospy.loginfo(result)
if result.word == 'start' and _inform_controller('imitate'):
state = 'imitate'
elif result.word == 'stop' and _inform_controller('stop'):
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'
continue
if not in_progress:
in_progress = True
client.send_goal(RequestSpeechGoal([voc_state[state]]),
done_cb)
rospy.Rate(2).sleep()

53
script/speech.py → script/speech_server.py
View File

@@ -5,24 +5,29 @@ import rospy
import actionlib
from naoqi import ALProxy, ALModule, ALBroker
from teleoperation.msg import RequestSpeechAction, RequestSpeechResult
speech_broker = None
almem = None
sas = actionlib.SimpleActionServer()
r = False
def request_speech(goal):
recognized = ''
if not speech_detector.start_speech(goal):
sas.publish_feedback(False)
if not speech_detector.start_speech(goal.vocabulary):
sas.set_succeeded(RequestSpeechResult(word=''))
return
while not sas.is_preempt_requested() and not speech_detector.have_word():
rospy.Rate(10).sleep()
speech_detector.stop_speech(goal)
if speech_detector.have_word():
recognized = speech_detector.get_recognized_and_erase()
sas.publish_feedback(True)
sas.set_succeeded(recognized)
else:
speech_detector.stop_speech()
recognized = ''
sas.set_succeeded(RequestSpeechResult(word=recognized))
class SpeechDetectorModule(ALModule):
@@ -31,14 +36,16 @@ class SpeechDetectorModule(ALModule):
ALModule.__init__(self, name)
self.recognized = None
self.subid = 'teleoperation_speech'
self.voc = []
self.asr = ALProxy('ALSpeechRecognition')
self.tts = ALProxy('ALTextToSpeech')
self.asr.setLanguage('English')
self.stop_speech()
self.running = False
def start_speech(self, voc):
if self.running:
return False
# self.stop_speech()
self.voc = voc
self.asr.setVocabulary(voc, False)
self.asr.subscribe(self.subid)
self.asr.pause(False)
@@ -54,16 +61,27 @@ class SpeechDetectorModule(ALModule):
return result
def stop_speech(self):
self.asr.pause(True)
if not self.running:
return
self.asr.unsubscribe(self.subid)
self.asr.pause(True)
self.running = False
def on_word_recognized(self, *_args):
rospy.loginfo(almem.getData('WordRecognized'))
word, conf = almem.getData('WordRecognized')
if conf > 0.4:
self.stop_speech()
self.tts.say(word)
self.recognized = word
else:
self.stop_speech()
self.tts.say('I didn\'t understand. Please repeat')
self.start_speech(self.voc)
if __name__ == '__main__':
rospy.init_node('speech')
rospy.init_node('speech_server')
speech_broker = ALBroker('speech_broker', '0.0.0.0', 0,
os.environ['NAO_IP'], 9559)
speech_detector = SpeechDetectorModule('speech_detector')
@@ -71,11 +89,16 @@ if __name__ == '__main__':
almem.subscribeToEvent("WordRecognized",
"speech_detector",
"on_word_recognized")
speech_detector.start_speech(['start', 'stop', 'pause'])
speech_detector.asr.pause(True)
sas = actionlib.SimpleActionServer('speech_server', RequestSpeechAction,
execute_cb=request_speech,
auto_start=False)
sas.start()
while not rospy.is_shutdown():
rospy.Rate(4).sleep()
speech_detector.stop_speech()
if speech_detector.running:
speech_detector.stop_speech()
speech_broker.shutdown()

40
script/walker.py
View File

@@ -1,5 +1,6 @@
#! /usr/bin/env python
import os
import json
from time import sleep
import rospy
@@ -9,18 +10,36 @@ from naoqi import ALProxy
from controller import inform_controller_factory
#min #max
FW = -1.70, -1.40
BK = -2.20, -2.40
LT = 0.35, 0.53
RT = -0.35, -0.53
LR = 0.45, 0.85
RR = -0.45, -0.85
FW = None
BK = None
LT = None
RT = None
LR = None
RR = None
VMIN = 0.3
VMAX = 1.0
def thirdway(a, b):
return a + (b - a) / 3
def global_init():
global FW, BK, LT, RT, LR, RR
here = os.path.dirname(os.path.realpath(__file__))
with open('{}/../config/default.yaml'.format(here)) as f:
x = json.load(f)
cx, cy, cz = x['cr']
FW = thirdway(cx, x['fw']), x['fw']
BK = thirdway(cx, x['bk']), x['bk']
LT = thirdway(cy, x['lt']), x['lt']
RT = thirdway(cy, x['rt']), x['rt']
LR = thirdway(cz, x['lr']), x['lr']
RR = thirdway(cz, x['rr']), x['rr']
_inform_controller = inform_controller_factory('walker')
@@ -37,11 +56,12 @@ def _speed(pos, interval):
if __name__ == '__main__':
rospy.init_node('teleoperation/walker')
rospy.init_node('walker')
global_init()
rospy.wait_for_service('inform_controller')
ll = tf.TransformListener()
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
# mp.wakeUp()
mp.wakeUp()
mp.moveInit()
mp.setMoveArmsEnabled(False, False)
@@ -84,7 +104,7 @@ if __name__ == '__main__':
if not permission:
mp.stopMove()
else:
# mp.moveToward(*movement) # DON'T DELETE THIS ONE
mp.moveToward(*movement) # DON'T DELETE THIS ONE
pass
mp.rest()