173 lines
4.9 KiB
Python
Executable File
173 lines
4.9 KiB
Python
Executable File
#! /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__':
|
|
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')
|