Started implementing main controller

Also sort of implemented the navigation
Also tidy up the CMakeLists a little

script/controller.py

@@ -0,0 +1,34 @@
+from time import sleep
+
+
+handlers = {}
+STATE = 'idle'  # Also walk, imitate and fallen
+
+
+def fall_handler():
+    pass
+
+
+def walk_handler():
+    pass
+
+
+def imitation_handler():
+    pass
+
+
+def handle_transition(new_state):
+    global STATE
+    if STATE == 'walk':
+        if new_state in ('fallen', 'idle'):
+            STATE = new_state
+    elif STATE == 'fallen':
+        if new_state == 'idle':
+            STATE = new_state
+    elif STATE == 'imitate':
+        STATE = new_state
+
+
+if __name__ == '__main__':
+    pass
+    # initialize stuff

script/walker.py

@@ -0,0 +1,55 @@
+#! /usr/bin/env python
+import os
+from time import sleep
+
+from naoqi import ALProxy
+
+import rospy
+import tf
+
+FW = -0.30
+BK = -0.55
+LT = -0.55
+RT = 0.0
+
+
+if __name__ == '__main__':
+    rospy.init_node('walker')
+    ll = tf.TransformListener()
+    mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+    mp.wakeUp()
+    mp.moveInit()
+
+    while not rospy.is_shutdown():
+        sleep(0.3)
+        try:
+            trans, rot = ll.lookupTransform('Aruco_0_frame',
+                                            'CameraTop_optical_frame',
+                                            rospy.Time(0))
+        except Exception as e:
+            mp.stopMove()
+            # inform_controller('walker', 'stop')
+            continue
+        print trans
+        # continue
+
+        if BK < trans[2] < FW and LT < trans[0] < RT:
+            mp.move(0, 0, 0)
+            # inform_controller('walker', 'stop')
+            continue
+
+        permission = True
+        # permission = inform_controller('walker', 'move')
+        if not permission:
+            mp.stopMove()
+            continue
+        if trans[2] < BK:  # backwards
+            mp.move(-1, 0, 0)
+        elif FW < trans[2]:  # forwards
+            mp.move(1, 0, 0)
+        elif RT < trans[0]:  # right
+            mp.move(0, -1, 0)
+        elif trans[0] < LT:  # left
+            mp.move(0, 1, 0)
+
+    mp.rest()