did some good day's work
This commit is contained in:
Pavel Lutskov
@@ -1,7 +1,6 @@
|
||||
#! /usr/bin/env python
|
||||
import os
|
||||
import json
|
||||
from sys import argv
|
||||
|
||||
import rospy
|
||||
import tf
|
||||
@@ -9,167 +8,127 @@ import actionlib
|
||||
import numpy as np
|
||||
from naoqi import ALProxy
|
||||
|
||||
from masterloop import inform_masterloop_factory
|
||||
from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal
|
||||
|
||||
|
||||
_inform_masterloop = inform_masterloop_factory('calibrator')
|
||||
def calib_field(sentences, src_frames, targ_frames):
|
||||
ok = False
|
||||
t_and_q = []
|
||||
|
||||
while not ok:
|
||||
for s in sentences:
|
||||
tts.say(s)
|
||||
tts.say('Then say start')
|
||||
client.send_goal(RequestSpeechGoal(['start']))
|
||||
client.wait_for_result()
|
||||
for src, targ in zip(src_frames, targ_frames):
|
||||
try:
|
||||
t_and_q.append(ll.lookupTransform(src, targ, rospy.Time(0)))
|
||||
ok = True
|
||||
except:
|
||||
ok = False
|
||||
tts.say('Step failed. Try again')
|
||||
break
|
||||
|
||||
return t_and_q
|
||||
|
||||
|
||||
def calib_center():
|
||||
trans, q = calib_field([
|
||||
'Stand in front of camera',
|
||||
'Far enough to see your arms'
|
||||
], ['odom'], ['Aruco_0_frame'])[0]
|
||||
rot = tf.transformations.euler_from_quaternion(q)
|
||||
return trans, rot
|
||||
|
||||
|
||||
def calib_arms():
|
||||
ts_and_qs = calib_field(
|
||||
['Now stretch your arms', 'Make sure the markers are detected'],
|
||||
['odom'] * 3,
|
||||
['Aruco_{}_frame'.format(i) for i in range(3)]
|
||||
)
|
||||
a0 = ts_and_qs[0][0]
|
||||
a1 = ts_and_qs[1][0]
|
||||
a2 = ts_and_qs[2][0]
|
||||
return (
|
||||
tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
|
||||
tuple(c2 - c0 for c0, c2 in zip(a0, a2))
|
||||
)
|
||||
|
||||
|
||||
def calib_shoulders():
|
||||
ts_and_qs = calib_field(
|
||||
['Now place the markers on the corresponding shoulders',
|
||||
'Make sure the markers are detected'],
|
||||
['odom'] * 3,
|
||||
['Aruco_{}_frame'.format(i) for i in range(3)]
|
||||
)
|
||||
a0 = ts_and_qs[0][0]
|
||||
a1 = ts_and_qs[1][0]
|
||||
a2 = ts_and_qs[2][0]
|
||||
return (
|
||||
tuple(c1 - c0 for c0, c1 in zip(a0, a1)),
|
||||
tuple(c2 - c0 for c0, c2 in zip(a0, a2))
|
||||
)
|
||||
|
||||
|
||||
def calib_rotation():
|
||||
rots = []
|
||||
for side in ('left', 'right'):
|
||||
_, q = calib_field(
|
||||
['Now rotate to your {}'.format(side),
|
||||
'So that marker on the chest is still detected'],
|
||||
['odom'], ['Aruco_0_frame']
|
||||
)[0]
|
||||
rots.append(tf.transformations.euler_from_quaternion(q))
|
||||
return tuple(rots)
|
||||
|
||||
|
||||
def calib_extremes():
|
||||
transs = []
|
||||
for direction in ('forward', 'backward', 'to the left', 'to the right'):
|
||||
trans, _ = calib_field(
|
||||
['Now return to the initial position',
|
||||
'Take a big step {}'.format(direction)],
|
||||
['odom'],
|
||||
['Aruco_0_frame']
|
||||
)[0]
|
||||
transs.append(trans)
|
||||
|
||||
return tuple(transs)
|
||||
|
||||
|
||||
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()
|
||||
center = calib_center()
|
||||
larm, rarm = calib_arms()
|
||||
lsh, rsh = calib_shoulders()
|
||||
larm, rarm, lsh, rsh = larm[1:], rarm[1:], lsh[1:], rsh[1:]
|
||||
lr, rr = calib_rotation()
|
||||
fw, bk, lt, rt = calib_extremes()
|
||||
|
||||
try:
|
||||
trans, q = ll.lookupTransform('odom',
|
||||
'Aruco_0_frame',
|
||||
rospy.Time(0))
|
||||
rot = tf.transformations.euler_from_quaternion(q)
|
||||
center = trans, rot
|
||||
tts.say('Well done')
|
||||
|
||||
except Exception:
|
||||
pass
|
||||
calib = {
|
||||
'cr': [center[0][0], center[0][1], center[1][2]],
|
||||
'arm': (np.linalg.norm(np.array(larm) - np.array(lsh)) +
|
||||
np.linalg.norm(np.array(rarm) - np.array(rsh))) / 2,
|
||||
'lsh': [0.0] + list(lsh),
|
||||
'rsh': [0.0] + list(rsh),
|
||||
'rr': rr[2],
|
||||
'lr': lr[2],
|
||||
'fw': fw[0],
|
||||
'bk': bk[0],
|
||||
'lt': lt[1],
|
||||
'rt': rt[1],
|
||||
}
|
||||
|
||||
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 place the markers on the corresponding shoulders')
|
||||
tts.say('Make sure the markers are detected')
|
||||
tts.say('Then say start')
|
||||
client.send_goal(RequestSpeechGoal(['start']))
|
||||
client.wait_for_result()
|
||||
|
||||
try:
|
||||
lsh, _ = ll.lookupTransform('Aruco_0_frame',
|
||||
'Aruco_1_frame',
|
||||
rospy.Time(0))
|
||||
rsh, _ = 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')
|
||||
here = os.path.dirname(os.path.realpath(__file__))
|
||||
with open('{}/../config/default.yaml'.format(here), 'w') as f:
|
||||
json.dump(calib, f, indent=4)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if len(argv) < 2:
|
||||
raise SystemExit
|
||||
|
||||
rospy.init_node('calibrator', disable_signals=True)
|
||||
|
||||
@@ -184,5 +143,6 @@ if __name__ == '__main__':
|
||||
rospy.on_shutdown(lambda: client.cancel_goal())
|
||||
|
||||
calibration()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
rospy.signal_shutdown('Interrupted')
|
||||
|
||||
Reference in New Issue
Block a user