56 lines
1.3 KiB
Python
Executable File
56 lines
1.3 KiB
Python
Executable File
#! /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()
|