def __init__(self, robot, initial_pose, use_entry_points=False):
"""
Enter the arena by force driving through the door
:param robot: robot object
:param initial_pose:
:param use_entry_points:
"""
smach.StateMachine.__init__(self, outcomes=['done'])
self.robot = robot
with self:
# If the door is open, amigo will say that it goes to the registration table
smach.StateMachine.add(
"THROUGH_DOOR",
human_interaction.Say(robot, [
"I will start my task now", "Let's rock and roll!",
"Let's kick some ass!"
],
block=False),
transitions={"spoken": "FORCE_DRIVE_THROUGH_DOOR"})
smach.StateMachine.add('FORCE_DRIVE_THROUGH_DOOR',
self.ForceDrive(robot),
transitions={"done": "GO_TO_ENTRY_POINT"})
smach.StateMachine.add('GO_TO_ENTRY_POINT',
self.GoToEntryPoint(robot, initial_pose,
use_entry_points),
transitions={
"found": "done",
"not_found": "GO_TO_ENTRY_POINT",
"no_goal": "done",
"all_unreachable": "done"
})
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):
"""
Enter the arena by force driving through the door
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['done'])
self.robot = robot
with self:
# If the door is open, amigo will say that it goes to the registration table
smach.StateMachine.add(
"THROUGH_DOOR",
human_interaction.Say(robot, [
"I will start my task now", "Let's rock and roll!",
"Let's kick some ass!"
],
block=False),
transitions={"spoken": "FORCE_DRIVE_THROUGH_DOOR"})
smach.StateMachine.add('FORCE_DRIVE_THROUGH_DOOR',
ForceDrive(robot, 0.25, 0, 0, 5.0),
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"})