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.'