more documentation

This commit is contained in:
2019-02-28 15:00:40 +01:00
parent ef4dafdaf4
commit c2d25dfb4b
9 changed files with 84 additions and 13 deletions

View File

@@ -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