122 lines
3.2 KiB
Python
Executable File
122 lines
3.2 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""Module for walking in the environment (Human Joystick).
|
|
|
|
ROS Node: `walker`.
|
|
Uses `inform_masterloop` service.
|
|
|
|
"""
|
|
from __future__ import division
|
|
|
|
import os
|
|
import json
|
|
from time import sleep
|
|
|
|
import rospy
|
|
import tf
|
|
|
|
from masterloop import inform_masterloop_factory, mp
|
|
|
|
|
|
FW = None
|
|
BK = None
|
|
LT = None
|
|
RT = None
|
|
LR = None
|
|
RR = None
|
|
|
|
VMIN = 0.3
|
|
VMAX = 1.0
|
|
|
|
|
|
def n_way(a, b, n=3):
|
|
"""A point which is (b - a)/n away from a."""
|
|
return a + (b - a) / n
|
|
|
|
|
|
def global_init():
|
|
global FW, BK, LT, RT, LR, RR
|
|
here = os.path.dirname(os.path.realpath(__file__))
|
|
with open('{}/../config/default.yaml'.format(here)) as f:
|
|
x = json.load(f)
|
|
|
|
cx, cy, cz = x['cr']
|
|
FW = n_way(cx, x['fw']), x['fw']
|
|
BK = n_way(cx, x['bk']), x['bk']
|
|
LT = n_way(cy, x['lt']), x['lt']
|
|
RT = n_way(cy, x['rt']), x['rt']
|
|
LR = n_way(cz, x['lr'], 2), x['lr']
|
|
RR = n_way(cz, x['rr'], 2), x['rr']
|
|
|
|
|
|
_inform_masterloop = inform_masterloop_factory('walker')
|
|
|
|
|
|
def _speed(pos, interval):
|
|
"""Calculate speed based on the operators position."""
|
|
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')
|
|
global_init()
|
|
rospy.wait_for_service('inform_masterloop')
|
|
ll = tf.TransformListener()
|
|
mp.setMoveArmsEnabled(False, False)
|
|
mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)
|
|
|
|
while not rospy.is_shutdown():
|
|
sleep(0.3)
|
|
try:
|
|
trans, q = ll.lookupTransform('odom',
|
|
'Aruco_0_frame',
|
|
rospy.Time(0))
|
|
rot = tf.transformations.euler_from_quaternion(q)
|
|
|
|
except Exception as e:
|
|
mp.stopMove()
|
|
_inform_masterloop('stop')
|
|
continue
|
|
|
|
movement = [0, 0, 0]
|
|
ref_vec = trans[:2] + rot[2:]
|
|
#-1 1 -1 1 -1 1
|
|
for i, dr in enumerate((BK, FW, RT, LT, RR, LR)):
|
|
# figure out in which direction to move
|
|
idx = i // 2
|
|
sign = 1 if (i % 2) else -1
|
|
speed = _speed(ref_vec[idx], dr)
|
|
if speed:
|
|
movement[idx] = sign * speed
|
|
break
|
|
|
|
rospy.loginfo('WALKER: TRANS: {}'.format(trans))
|
|
rospy.loginfo('WALKER: ROTTN: {}'.format(rot))
|
|
rospy.loginfo('WALKER: MOVMT: {}\n'.format(movement))
|
|
|
|
if not any(movement):
|
|
rospy.logdebug('WALKER: STOP')
|
|
_inform_masterloop('stop') # release the control
|
|
# mp.move(0, 0, 0)
|
|
mp.stopMove()
|
|
continue
|
|
|
|
permission = _inform_masterloop('move') # try to seize the control
|
|
|
|
if not permission:
|
|
mp.stopMove()
|
|
else:
|
|
# set the head so that the scene is clearly visible on cameras
|
|
mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3)
|
|
mp.moveToward(*movement) # DON'T DELETE THIS ONE
|
|
pass
|
|
|
|
mp.rest()
|