def __init__(self, robot, initial_pose, use_entry_points = False, door=True): smach.StateMachine.__init__(self, outcomes=["Done", "Aborted", "Failed"]) assert hasattr(robot, "base") assert hasattr(robot, "speech") with self: smach.StateMachine.add( "NOTIFY_EBUTTON", check_ebutton.NotifyEButton(robot), transitions={ "succeeded" :"INITIALIZE"}) smach.StateMachine.add( "INITIALIZE", utility.Initialize(robot), transitions={ "initialized" :"INSTRUCT_WAIT_FOR_DOOR" if door else "INIT_POSE", "abort" :"Aborted"}) smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR", human_interaction.Say(robot, [ "Hi there, I will now wait until the door is opened", "I'm waiting for the door"], block=False), transitions={ "spoken":"WAIT_FOR_DOOR"}) # Start laser sensor that may change the state of the door if the door is open: smach.StateMachine.add( "WAIT_FOR_DOOR", WaitForDoorOpen(robot, timeout=10), transitions={ "closed":"DOOR_CLOSED", "open":"DOOR_OPEN"}) smach.StateMachine.add( "DOOR_CLOSED", human_interaction.Say(robot, ["Door is closed, please open the door", "I'd start, if only you'd let me in", "Please let me in"]), transitions={ "spoken":"WAIT_FOR_DOOR"}) smach.StateMachine.add( "DOOR_OPEN", human_interaction.Say(robot, "Door is open!", block=False), transitions={ "spoken":"INIT_POSE"}) # Initial pose is set after opening door, otherwise snapmap will fail if door is still closed and initial pose is set, # since it is thinks amigo is standing in front of a wall if door is closed and localization can(/will) be messed up. smach.StateMachine.add('INIT_POSE', utility.SetInitialPose(robot, initial_pose), transitions={ 'done':'ENTER_ROOM' if door else "Done", 'preempted':'Aborted', # This transition will never happen at the moment. 'error':'ENTER_ROOM' if door else "Done"}) # It should never go to aborted. # Enter the arena with force drive as back-up smach.StateMachine.add('ENTER_ROOM', EnterArena(robot, initial_pose, use_entry_points), transitions={ "done":"Done" })
def __init__(self, robot, initial_pose, use_entry_points=False, door=True): """ Initialization method :param robot: (Robot) :param initial_pose: (str) identifies the (waypoint) entity to be used as initial pose. For testing purposes, a tuple(float, float, float) representing x, y and yaw in map frame can be used. :param use_entry_points: (bool) (not yet implemented) :param door: (bool) indicates whether to wait for a door to open and whether to 'force-drive' inside """ smach.StateMachine.__init__(self, outcomes=["Done", "Aborted", "Failed"]) assert hasattr(robot, "base") assert hasattr(robot, "speech") if use_entry_points: raise NotImplementedError( "Use entry points is not yet implemented in StartChallengeRobust/EnterArena" ) with self: smach.StateMachine.add("NOTIFY_EBUTTON", check_ebutton.NotifyEButton(robot), transitions={"succeeded": "INITIALIZE"}) smach.StateMachine.add("INITIALIZE", utility.Initialize(robot), transitions={ "initialized": "INSTRUCT_WAIT_FOR_DOOR" if door else "WAIT_FOR_LOCAL_PLANNER", "abort": "Aborted" }) # We wait till the local planner is ready before starting the challenge, # otherwise the robot will not drive. smach.StateMachine.add("WAIT_FOR_LOCAL_PLANNER", WaitForLocalPlanner(robot, 10), transitions={ "ready": "INIT_POSE", "timeout": "Aborted" }) smach.StateMachine.add( "INSTRUCT_WAIT_FOR_DOOR", human_interaction.Say(robot, [ "Hi there, I will now wait until the door is opened", "I'm waiting for the door" ], block=False), transitions={"spoken": "WAIT_FOR_DOOR"}) # Start laser sensor that may change the state of the door if the door is open: smach.StateMachine.add("WAIT_FOR_DOOR", WaitForDoorOpen(robot, timeout=10), transitions={ "closed": "DOOR_CLOSED", "open": "DOOR_OPEN" }) smach.StateMachine.add( "DOOR_CLOSED", human_interaction.Say(robot, [ "Door is closed, please open the door", "I'd start, if only you'd let me in", "Please let me in" ]), transitions={"spoken": "WAIT_FOR_DOOR"}) smach.StateMachine.add("DOOR_OPEN", human_interaction.Say(robot, "Door is open!", block=False), transitions={"spoken": "INIT_POSE"}) # Initial pose is set after opening door, otherwise snapmap will fail if door is still closed and initial # pose is set, since it is thinks the robot is standing in front of a wall if door is closed and # localization can(/will) be messed up. smach.StateMachine.add( 'INIT_POSE', utility.SetInitialPose(robot, initial_pose), transitions={ 'done': 'ENTER_ROOM' if door else "Done", 'preempted': 'Aborted', # This transition will never happen at the moment. # It should never go to aborted. 'error': 'ENTER_ROOM' if door else "Done" }) # Enter the arena with force drive as back-up smach.StateMachine.add('ENTER_ROOM', EnterArena(robot), transitions={"done": "Done"})