Started implementing main controller

Also sort of implemented the navigation Also tidy up the CMakeLists a little
2019-01-17 16:15:05 +01:00
parent becede0124
commit fe09cce493
5 changed files with 100 additions and 192 deletions
--- a/script/walker.py
+++ b/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()