did some work

script/calibrator.py

@@ -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')