more documentation
This commit is contained in:
@@ -1,4 +1,10 @@
|
||||
#! /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
|
||||
@@ -23,6 +29,7 @@ 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
|
||||
|
||||
|
||||
@@ -45,6 +52,7 @@ _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
|
||||
@@ -81,6 +89,7 @@ if __name__ == '__main__':
|
||||
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)
|
||||
@@ -94,16 +103,17 @@ if __name__ == '__main__':
|
||||
|
||||
if not any(movement):
|
||||
rospy.logdebug('WALKER: STOP')
|
||||
_inform_masterloop('stop')
|
||||
_inform_masterloop('stop') # release the control
|
||||
# mp.move(0, 0, 0)
|
||||
mp.stopMove()
|
||||
continue
|
||||
|
||||
permission = _inform_masterloop('move')
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user