66 lines
1.5 KiB
Python
Executable File
66 lines
1.5 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
from time import sleep
|
|
|
|
import rospy
|
|
import tf
|
|
from naoqi import ALProxy
|
|
|
|
from controller import inform_controller_factory
|
|
|
|
FW = -0.32
|
|
BK = -0.55
|
|
LT = -0.55
|
|
RT = 0.0
|
|
|
|
|
|
_inform_controller = inform_controller_factory('walker')
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('walker')
|
|
rospy.wait_for_service('inform_controller')
|
|
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()
|
|
print _inform_controller('stop')
|
|
continue
|
|
|
|
print trans, rot
|
|
# continue
|
|
|
|
if (
|
|
BK < trans[2] < FW and
|
|
LT < trans[0] < RT
|
|
# CW < trans[1] < CC
|
|
):
|
|
mp.move(0, 0, 0)
|
|
_inform_controller('stop')
|
|
continue
|
|
|
|
permission = _inform_controller('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()
|