def main():
__init__()
#HeadKeepAliveAnim.start()
#rospy.timer.sleep(45)
#HeadKeepAliveAnim.stop()
# Obtain a queue from which human distances are obtained as
# they change:
eventQueue = human.getEventQueue()
# Motions, like the function "longMotionSequence" above are
# started using the following statement. Note that you
# can currently only have one motion sequence running at
# the same time. You can stop a motion anytime by calling
# myMotion.stop(). For pausing a motion, see below:
NO_HUMANS = 'NO_HUMANS'
VISIBLE_HUMANS = 'VISIBLE_HUMANS'
CLOSE_HUMANS = 'CLOSE_HUMANS'
state = NO_HUMANS
defaulPoseAnim.start()
while not defaulPoseAnim.is_done():
time.sleep(0.01)
defaulPoseAnim.stop()
#headKeepAlive(None)
# Two seconds between the two head motions:
# This is an example for a parallel function
# taking an argument:
#headKeepAliveAnim.start(5)
while (True):
# Wait for a new position report from the human:
newHumanLocation = eventQueue.get()
humanDistance = pr2.getPlanarDistance(newHumanLocation)
humanAngle = pr2.getAngle(newHumanLocation)
pr2.displayInfo('Robot state:' + state)
## UPDATE THE STATE
if state == NO_HUMANS:
## In NO_HUMANS
pr2.lookAtPoint(newHumanLocation, 1.0, wait=False)
if humanDistance < 4:
state = VISIBLE_HUMANS
print('Current state ' + state)
## NO_HUMANS --> VISIBLE_HUMANS
animation4.stop()
animation3.start()
elif state == VISIBLE_HUMANS:
## In CLOSE_HUMANS
pr2.lookAtPoint(newHumanLocation, 1.0, wait=False)
if humanDistance < 2:
state = CLOSE_HUMANS
print('Current state ' + state)
## VISIBLE_HUMANS --> CLOSE_HUMANS
animation3.stop()
animation2.start()
elif humanDistance > 4.5:
state = NO_HUMANS
print('Current state ' + state)
## VISIBLE_HUMANS --> NO_HUMANS
animation3.stop()
animation4.start()
elif state == CLOSE_HUMANS:
## In CLOSE_HUMANS
if humanDistance > 2.5:
state = VISIBLE_HUMANS
print('Current state ' + state)
## CLOSE_HUMANS --> VISIBLE_HUMANS
animation2.stop()
animation3.start()
else:
print 'ERROR: Unknown state.'
