86 lines
2.1 KiB
Python
Executable File
86 lines
2.1 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
|
|
|
|
|
|
#min #max
|
|
FW = 1.65, 1.45
|
|
BK = 2.20, 2.40
|
|
LT = -0.35, -0.53
|
|
RT = 0.35, 0.53
|
|
|
|
VMIN = 0.3
|
|
VMAX = 1.0
|
|
|
|
|
|
_inform_controller = inform_controller_factory('walker')
|
|
|
|
|
|
def _speed(pos, interval):
|
|
int_dir = 1 if interval[1] > interval[0] else -1
|
|
if int_dir * (pos - interval[0]) < 0:
|
|
return 0.0
|
|
elif int_dir * (pos - interval[1]) > 0:
|
|
return 1.0
|
|
else:
|
|
return (VMAX - VMIN) * abs(pos - interval[0]) / (
|
|
abs(interval[1] - interval[0])
|
|
) + VMIN
|
|
|
|
|
|
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()
|
|
mp.setMoveArmsEnabled(False, False)
|
|
|
|
while not rospy.is_shutdown():
|
|
sleep(0.3)
|
|
try:
|
|
trans, rot = ll.lookupTransform('Aruco_0_frame',
|
|
'odom',
|
|
rospy.Time(0))
|
|
except Exception as e:
|
|
mp.stopMove()
|
|
_inform_controller('stop')
|
|
continue
|
|
|
|
# print trans
|
|
# continue
|
|
|
|
permission = _inform_controller('move')
|
|
|
|
if not permission:
|
|
mp.stopMove()
|
|
continue
|
|
|
|
movement = [0, 0, 0]
|
|
#-1 1 -1 1
|
|
for i, dr in enumerate((BK, FW, RT, LT)):
|
|
idx = i // 2
|
|
sign = 1 if (i % 2) else -1
|
|
speed = _speed(trans[idx], dr)
|
|
if speed:
|
|
movement[idx] = sign * speed
|
|
break
|
|
|
|
if not any(movement):
|
|
rospy.loginfo('WALKER: STOP')
|
|
_inform_controller('stop')
|
|
mp.move(0, 0, 0)
|
|
else:
|
|
rospy.loginfo('WALKER: TRANS: {}'.format(trans))
|
|
rospy.loginfo('WALKER: MOVMT: {}'.format(movement))
|
|
mp.moveToward(*movement)
|
|
|
|
mp.rest()
|