def __init__(self, robot, order_type):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
beverage_dest_desig = EdEntityDesignator(robot) #.id is overwritten by instruct_barman
with self:
@smach.cb_interface(outcomes=['spoken'])
def instruct_barman(userdata=None):
try:
order = ORDERS[order_type]
beverage_dest_desig.id = order['location']
robot.speech.speak("Barman, please put a {name} in my basket for table {location}".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'INSTRUCT_BARMAN',
smach.CBState(instruct_barman),
transitions={'spoken' :'AWAIT_PUT_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_PUT_ORDER_CONFIRMATION',
states.WaitTime(robot, 8),
transitions={ 'waited' :'GOTO_ORDER_DESTINATION_1',
'preempted' :'failed'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_1', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'GOTO_ORDER_DESTINATION_2',
'goal_not_defined' :'GOTO_ORDER_DESTINATION_2'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_2', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
@smach.cb_interface(outcomes=['spoken'])
def instruct_guest(userdata=None):
try:
order = ORDERS[order_type]
robot.speech.speak("Dear guest at table {location}, you can get your {name} from my basket.".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'SAY_TAKE_ORDER',
smach.CBState(instruct_guest),
transitions={'spoken' :'AWAIT_TAKE_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_TAKE_ORDER_CONFIRMATION',
states.WaitTime(robot, 5),
transitions={ 'waited' :'SAY_ENJOY_ORDER',
'preempted' :'failed'})
smach.StateMachine.add( 'SAY_ENJOY_ORDER',
states.Say(robot, ["Enjoy your {}".format(order_type)], block=False),
transitions={ 'spoken' :'succeeded'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done'])
sentences = ["%s, please clean this object %s the %s" % (other_robot_name, segment_area, location_id),
"%s, can you clean the trash %s the %s?" % (other_robot_name, segment_area, location_id),
"Can another robot clean the garbage %s the %s?" % (segment_area, location_id)]
with self:
smach.StateMachine.add('SAY_OTHER_ROBOT_CLEANUP',
robot_smach_states.Say(robot, sentences, block=False),
transitions={"spoken": "NAVIGATE_TO_INITIAL"})
smach.StateMachine.add( "NAVIGATE_TO_INITIAL",
robot_smach_states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_point,), radius=0.05),
transitions={ 'arrived' : 'CONTACT_OTHER_ROBOT',
'unreachable' : 'CONTACT_OTHER_ROBOT',
'goal_not_defined' : 'CONTACT_OTHER_ROBOT'})
smach.StateMachine.add('CONTACT_OTHER_ROBOT',
ContactOtherRobot(robot, selected_entity_designator),
transitions={"done": "WAIT_FOR_TRIGGER", "failed": "SAY_FAILED"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(robot, ["continue", "gpsr"], "/amigo/trigger"),
transitions={"continue": "SAY_DONE", "gpsr": "SAY_FAILED", "preempted" : "SAY_FAILED"})
smach.StateMachine.add('SAY_DONE',
robot_smach_states.Say(robot, ["Thanks for cleaning", "Thank you", "You are the best"], block=True),
transitions={"spoken": "done"})
smach.StateMachine.add('SAY_FAILED',
robot_smach_states.Say(robot, ["Too bad then, we will just leave that trash there"], block=True),
transitions={"spoken": "done"})
def __init__(self, robot, door_start_wp_designator, door_dest_wp_designator, approach_speed=0.1, push_speed=0.05, attempts=10):
"""
Push against a door until its open
:param robot: Robot on which to execute this state machine
:param door_entity_designator: The door entity. Defaults to None, which implies the door its in front of
This entity must have 2 fields in its data: push_start_waypoint and push_destination_waypoint,
which must both contain an ID of a waypoint.
push_start_waypoint is from where the robot will start pushing
push_destination_waypoint simply needs to be reachable for the door the be considered 'open'
:param approach_speed: Speed with which to approach the door
:param push_speed: Speed with which to push against the door
:return:
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
#door_start_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='start', name='door_open_start')
#door_dest_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='destination', name='door_open_dest')
with self:
smach.StateMachine.add( 'GOTO_DOOR_START',
states.NavigateToWaypoint(robot, door_start_wp_designator, radius=0.05),
transitions={'arrived' :'PUSH_DOOR_ITERATOR',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
# START REPEAT DOOR OPENING
push_door_iterator = smach.Iterator(outcomes=['open', 'closed', 'failed'],
it = lambda:range(0, attempts),
it_label='counter',
input_keys=[],
output_keys=[],
exhausted_outcome = 'failed')
with push_door_iterator:
push_door = smach.StateMachine( outcomes = ['open', 'closed', 'failed'])
with push_door:
smach.StateMachine.add( 'APPROACH_AGAINST_N',
ForceDriveToTouchDoor(robot, approach_speed),
transitions={'front':'closed',
'left':'CHECK_DOOR_PASSABLE',
'right':'CHECK_DOOR_PASSABLE',
'failed':'failed'})
smach.StateMachine.add( 'CHECK_DOOR_PASSABLE',
CheckDoorPassable(robot,
destination_designator=door_dest_wp_designator),
transitions={'blocked':'closed',
'passable':'open'})
smach.Iterator.set_contained_state( 'APPROACH_AGAINST',
push_door,
loop_outcomes=['failed', 'closed'],
break_outcomes=['open'])
smach.StateMachine.add( 'PUSH_DOOR_ITERATOR',
push_door_iterator,
{ 'open':'succeeded',
'closed':'failed',
'failed':'failed'})
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(robot,
"Starting navigation test",
block=False),
transitions={'spoken': 'SAY_TEST_SYMBOLIC'})
smach.StateMachine.add('SAY_TEST_SYMBOLIC',
states.Say(robot,
"Testing Navigate To Symbolic",
block=False),
transitions={'spoken': 'NAV_TO_SYMB'})
smach.StateMachine.add(
'NAV_TO_SYMB',
states.NavigateToSymbolic(
robot,
{EntityByIdDesignator(robot, id="living_room"): "in"},
EntityByIdDesignator(robot, id="dinnertable")),
transitions={
'arrived': 'SAY_TEST_WAYPOINT',
'unreachable': 'SAY_FAILED_SYMBOLIC',
'goal_not_defined': 'SAY_FAILED_SYMBOLIC'
})
smach.StateMachine.add('SAY_FAILED_SYMBOLIC',
states.Say(robot,
"Failed Navigate To Symbolic.",
block=True),
transitions={'spoken': 'SAY_TEST_WAYPOINT'})
smach.StateMachine.add('SAY_TEST_WAYPOINT',
states.Say(robot,
"Testing Navigate To Waypoint.",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'container_success',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'container_success'})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
runs = ds.Designator([0, 1])
run = ds.VariableDesignator(resolve_type=int)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'HANDLE_GUEST_1',
"preempted": 'aborted',
'error': 'HANDLE_GUEST_1'
})
smach.StateMachine.add('HANDLE_GUEST_1',
HandleSingleGuest(robot, assume_john=True),
transitions={
'succeeded': 'HANDLE_GUEST_2',
'aborted': 'HANDLE_GUEST_2'
})
smach.StateMachine.add('HANDLE_GUEST_2',
HandleSingleGuest(robot, assume_john=False),
transitions={
'succeeded': 'SAY_DONE',
'aborted': 'SAY_DONE'
})
smach.StateMachine.add(
'SAY_DONE',
states.Say(robot,
["That's all folks, my job is done, bye bye!"],
block=False),
transitions={'spoken': 'GO_BACK'})
smach.StateMachine.add(
'GO_BACK',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id']),
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'succeeded',
'unreachable': 'succeeded',
'goal_not_defined': 'succeeded'
})
def __init__(self, robot, drop_zone_id):
"""
:param robot: robot object
:param drop_designator: EdEntityDesignator designating the collection zone
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
with self:
smach.StateMachine.add("GO_TO_COLLECTION_ZONE",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "OPEN_DOOR_PLEASE"})
smach.StateMachine.add("OPEN_DOOR_PLEASE",
states.Say(robot, "Can you please open the door for me? It seems blocked!"),
transitions={"spoken": "WAIT_FOR_DOOR_OPEN"})
smach.StateMachine.add("WAIT_FOR_DOOR_OPEN",
states.WaitTime(robot=robot, waittime=5),
transitions={"waited": "GO_TO_COLLECTION_ZONE2",
"preempted": "GO_TO_COLLECTION_ZONE2"})
smach.StateMachine.add("GO_TO_COLLECTION_ZONE2",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "HANDOVER"})
smach.StateMachine.add("HANDOVER",
states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"})
def execute(self, userdata):
target_waypoint = challenge_knowledge.waypoints[userdata.target_room]['id']
target_radius = challenge_knowledge.waypoints[userdata.target_room]['radius']
navigateToWaypoint = states.NavigateToWaypoint(self._robot,
ds.EntityByIdDesignator(self._robot,
id=target_waypoint),
target_radius)
return navigateToWaypoint.execute()
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(
robot,
"Starting people recognition test",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'LOOK_AT_OPERATOR',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
smach.StateMachine.add('LOOK_AT_OPERATOR',
states_interaction.LookAtPersonInFront(
robot, lookDown=False),
transitions={
'succeeded': 'RECOGNIZE_PEOPLE',
'failed': 'RECOGNIZE_PEOPLE'
})
smach.StateMachine.add('RECOGNIZE_PEOPLE',
test_states.RecognizePeople(robot),
transitions={
'succeeded': 'container_success',
'failed': 'SAY_NO_PEOPLE',
'no_people': 'SAY_NO_PEOPLE'
})
smach.StateMachine.add('SAY_NO_PEOPLE',
states.Say(
robot,
"I don see anyone in front of me.",
block=False),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
def test_delivery(robot):
from robot_skills.util.kdl_conversions import kdl_frame_stamped_from_XYZRPY
robot.ed.update_entity(id="one",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=1.0, y=0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="two",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=-1.2, y=0.0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="three",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=1.950, y=1.551, frame_id="/map"),
type="waypoint")
global ORDERS
ORDERS = {
"beverage": {
"name": "coke",
"location": "one"
},
"combo": {
"name": "pringles and chocolate",
"location": "two"
}
}
deliver = smach.StateMachine(outcomes=['done', 'aborted'])
with deliver:
smach.StateMachine.add('DELIVER_BEVERAGE',
DeliverOrderWithBasket(robot, "beverage"),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'NAVIGATE_TO_KITCHEN'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot, id="kitchen"),
radius=WAYPOINT_RADIUS),
transitions={
'arrived': 'DELIVER_COMBO',
'unreachable': 'DELIVER_COMBO',
'goal_not_defined': 'DELIVER_COMBO'
})
smach.StateMachine.add('DELIVER_COMBO',
DeliverOrderWithBasket(robot, "combo"),
transitions={
'succeeded': 'done',
'failed': 'aborted'
})
deliver.execute(None)
def __init__(self, robot, area, name):
""" Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot,
id=area + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=area)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(
robot=robot,
waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={
"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=waypoint_designator,
radius=0.15),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(
robot=robot,
entity_designator_room=room_designator),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PERSON",
FindPerson(robot=robot, person_label=name),
transitions={
"found": "found",
"failed": "not_found"
})
def __init__(self, robot, room, found_people_designator,
look_range=(-np.pi/2, np.pi/2),
look_steps=8):
"""
Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
:param discard_other_labels: (bool) Whether or not to discard faces based on label
:param found_person_designator: (Designator) A designator that will resolve to the found object
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=room)
ds.check_type(found_people_designator, [Entity])
ds.is_writeable(found_people_designator)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(robot=robot,
waypoint_designator=waypoint_designator, radius=0.15),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PEOPLE",
FindPeople(robot=robot,
query_entity_designator=room_designator,
found_people_designator=found_people_designator,
speak=True,
look_range=look_range,
look_steps=look_steps),
transitions={"found": "found",
"failed": "not_found"})
def __init__(self, robot, personNameDesLocal):
@smach.cb_interface(outcomes=['spoken'])
def saySearchingOperatorCB(userdata=None):
printOk("saySearchingOperatorCB")
robot.speech.speak("I am searching for " +
personNameDesLocal.resolve() + "!",
block=False)
return 'spoken'
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'SAY_SEARCHING_OPERATOR',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add(
'SAY_FAILED_WAYPOINT',
states.Say(robot, "Failed reaching the waypoint.", block=True),
transitions={'spoken': 'SAY_SEARCHING_OPERATOR'})
smach.StateMachine.add('SAY_SEARCHING_OPERATOR',
smach.CBState(saySearchingOperatorCB),
transitions={'spoken': 'SAY_UNFINSHED'})
smach.StateMachine.add('SAY_UNFINSHED',
states.Say(robot,
"This part is not finished yet.",
block=False),
transitions={'spoken': 'container_success'})
def __init__(self, robot):
"""
Drives to the lightsaber pose and starts the lightsaber state.
"""
smach.StateMachine.__init__(self, outcomes=["done"])
with self:
smach.StateMachine.add(
"NAVIGATE_TO_START",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(
robot, id=LIGHTSABER_WAYPOINT_ID),
),
transitions={
"arrived": "SWORDFIGHT",
"unreachable": "SWORDFIGHT", # Just take it from here
"goal_not_defined": "SWORDFIGHT"
}) # Just take it from here
smach.StateMachine.add("SWORDFIGHT",
LightSaber(robot),
transitions={"done": "done"})
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"
})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
states.AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def __init__(self, robot):
""" Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
# Create designators
trashbin_designator = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id,
name='trashbin_designator')
# Look if there is a second trash bin present
# trashbin_designator2 = None
if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"):
trashbin_designator2 = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator2')
next_state = "HELPER_WAYPOINT"
rospy.loginfo("There is a second trash bin")
else:
rospy.loginfo("There is no second trash bin")
next_state = "ANNOUNCE_END"
# drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
helper_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
end_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
with self:
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
CHALLENGE_KNOWLEDGE.starting_point),
transitions={
"Done": "SAY_START_CHALLENGE",
"Aborted": "SAY_START_CHALLENGE",
"Failed": "SAY_START_CHALLENGE"
})
smach.StateMachine.add("SAY_START_CHALLENGE",
states.Say(
robot,
"I will start cleaning up the trash",
block=True),
transitions={'spoken': "PICK_UP_TRASH"})
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(
robot=robot,
trashbin_designator=trashbin_designator,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH",
"failed": "HELPER_WAYPOINT",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH",
DropDownTrash(robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK",
states.Say(robot,
"First bag has been dropped at the collection zone",
block=False),
transitions={'spoken': next_state})
if next_state == "HELPER_WAYPOINT":
smach.StateMachine.add(
"HELPER_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=helper_waypoint_designator),
transitions={
"arrived": "PICK_UP_TRASH2",
"goal_not_defined": "PICK_UP_TRASH2",
"unreachable": "PICK_UP_TRASH2"
})
smach.StateMachine.add(
"PICK_UP_TRASH2",
PickUpTrash(robot=robot,
trashbin_designator=trashbin_designator2,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH2",
"failed": "ANNOUNCE_END",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH2",
DropDownTrash(
robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK2",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK2",
states.Say(
robot,
"Second bag has been dropped at the collection zone."
"All the thrash has been taken care of",
block=False),
transitions={'spoken': 'ANNOUNCE_END'})
smach.StateMachine.add("ANNOUNCE_END",
states.Say(
robot,
"I have finished taking out the trash.",
block=False),
transitions={'spoken': 'NAVIGATE_OUT'})
smach.StateMachine.add(
"NAVIGATE_OUT",
states.NavigateToWaypoint(
robot=robot, waypoint_designator=end_waypoint_designator),
transitions={
"arrived": "succeeded",
"goal_not_defined": "succeeded",
"unreachable": "succeeded"
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
with self:
#Part I: Set a table
smach.StateMachine.add(
'ENTER_ROOM', # Enter the room
states.StartChallengeRobust(robot, knowledge.initial_pose),
transitions={
'Done': 'ANNOUNCEMENT',
'Aborted': 'Aborted',
'Failed': 'Aborted'
})
smach.StateMachine.add(
'ANNOUNCEMENT',
states.Say(
robot,
"Let's see if my master has a task for me! Moving to the meeting point.",
block=True),
transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_I'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_I',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3),
transitions={
'arrived': 'FETCH_COMMAND_I',
'unreachable': 'FETCH_COMMAND_I',
'goal_not_defined': 'FETCH_COMMAND_I'
})
smach.StateMachine.add(
'FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0),
transitions={'heard': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot,
"What should I serve, master?",
block=True),
transitions={'spoken': 'SET_THE_TABLE'})
smach.StateMachine.add(
'SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
states.Initialize(robot),
transitions={
'initialized': 'SERVE_MEAL',
'abort': 'Aborted'
},
remapping={'meal': 'meal'})
smach.StateMachine.add(
'SERVE_MEAL', # Serve the meal (for example: pour milk into the bowl)
states.Initialize(robot),
transitions={
'initialized': 'CORRECT_OBJECT_POSITIONS',
'abort': 'Aborted'
})
smach.StateMachine.add(
'CORRECT_OBJECT_POSITIONS', # Inspect table and correct the moved objects
states.Initialize(robot),
transitions={
'initialized': 'ANNOUNCE_TASK_COMPLETION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ANNOUNCE_TASK_COMPLETION',
states.Say(
robot,
"The table is set! Moving to the meeting point for the next task.",
block=True),
transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_II'})
#Part II: Clean the table
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_II',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3),
transitions={
'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'
})
smach.StateMachine.add(
'FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0),
transitions={'heard': 'CLEAR_UP'})
smach.StateMachine.add(
'CLEAR_UP', # Clear up the table (bring the objects to their default location)
states.Initialize(robot),
transitions={
'initialized': 'CLEAN_THE_TABLE',
'abort': 'Aborted'
})
smach.StateMachine.add(
'CLEAN_THE_TABLE', # Inspect for spots and spills and clean them
states.Initialize(robot),
transitions={
'initialized': 'END_CHALLENGE',
'abort': 'Aborted'
})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am finally free!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_ROBOCUP_ARENA',
'abort': 'aborted'
})
smach.StateMachine.add('STORE_ROBOCUP_ARENA',
StoreRobocupArena(robot),
transitions={'done': 'HEAD_STRAIGHT'})
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'})
smach.StateMachine.add('SAY_INTRO',
states.Say(
robot,
"Hi, Guide me out of the arena please.",
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_INITIAL'})
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator':
'WAIT_FOR_OPERATOR_COMMAND',
'no_operator': 'FOLLOW_INITIAL'
})
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
learn_face=False,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator': 'SAY_GUIDE',
'no_operator': 'SAY_GUIDE'
})
smach.StateMachine.add('WAIT_FOR_OPERATOR_COMMAND',
WaitForOperatorCommand(robot),
transitions={
'follow': 'FOLLOW',
'command': 'SAY_GUIDE'
})
smach.StateMachine.add(
'SAY_GUIDE',
states.Say(robot,
"I will guide you back to the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'GUIDE_TO_ROBOCUP_ARENA'})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius),
transitions={
'arrived':
'SAY_BACK',
'unreachable':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP',
'goal_not_defined':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP'
})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius + 0.1),
transitions={
'arrived': 'SAY_BACK',
'unreachable': 'GUIDE_TO_ROBOCUP_ARENA',
'goal_not_defined': 'GUIDE_TO_ROBOCUP_ARENA'
})
smach.StateMachine.add('SAY_BACK',
states.Say(robot,
"We are back in the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'done'})
return sm
def setup_statemachine(robot):
load_waypoints(robot)
operator_id = VariableDesignator(resolve_type=str)
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'STORE_KITCHEN',
'abort': 'aborted'}
)
smach.StateMachine.add('STORE_KITCHEN',
StoreKitchen(robot),
transitions={'done': 'HEAD_STRAIGHT'}
)
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'}
)
smach.StateMachine.add('SAY_INTRO',
states.Say(robot, "Hi, Show me your restaurant please."),
transitions={'spoken': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('STORE',
StoreWaypoint(robot),
transitions={'done': 'CHECK_KNOWLEDGE',
'continue': 'FOLLOW'}
)
smach.StateMachine.add('CHECK_KNOWLEDGE',
CheckKnowledge(robot),
transitions={'yes': 'SAY_FOLLOW_TO_KITCHEN',
'no':'FOLLOW'}
)
smach.StateMachine.add('SAY_FOLLOW_TO_KITCHEN',
states.Say(robot,
"Please bring me back to the kitchen!"
),
transitions={'spoken': 'FOLLOW_TO_KITCHEN'}
)
smach.StateMachine.add('FOLLOW_TO_KITCHEN',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=False,
operator_id_des=operator_id
),
transitions={'stopped': 'CHECK_IN_KITCHEN',
'lost_operator': 'SAY_GOTO_KITCHEN',
'no_operator': 'SAY_GOTO_KITCHEN'}
)
smach.StateMachine.add('SAY_GOTO_KITCHEN',
states.Say(robot,
"You know what? I will go back to the kitchen on my own!",
block=False
),
transitions={'spoken': 'GOTO_KITCHEN'})
smach.StateMachine.add('GOTO_KITCHEN',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(robot, id="kitchen")
),
transitions={'arrived': 'SAY_IN_KITCHEN',
'unreachable': 'SAY_I_DONT_KNOW_HOW',
'goal_not_defined': 'SAY_I_DONT_KNOW_HOW'}
)
smach.StateMachine.add('SAY_I_DONT_KNOW_HOW',
states.Say(robot,
"Oops, I don't know the way back.",
block=True
),
transitions={'spoken': 'GOTO_KITCHEN'})
# smach.StateMachine.add('FOLLOW_TO_KITCHEN',
# states.FollowOperator(robot,
# operator_timeout=30,
# ask_follow=False
# ),
# transitions={'stopped': 'CHECK_IN_KITCHEN',
# 'lost_operator': 'FOLLOW_TO_KITCHEN_INITIAL',
# 'no_operator': 'FOLLOW_TO_KITCHEN_INITIAL'}
# )
smach.StateMachine.add('CHECK_IN_KITCHEN',
CheckInKitchen(robot),
transitions={'not_in_kitchen': 'FOLLOW_TO_KITCHEN',
'in_kitchen':'SAY_IN_KITCHEN'}
)
smach.StateMachine.add('SAY_IN_KITCHEN',
states.Say(robot,
"We are in the kitchen again!"
),
transitions={'spoken': 'SAY_WHICH_ORDER'}
)
# Where to take the order from?
smach.StateMachine.add('SAY_WHICH_ORDER', states.Say(robot, "From which table should I take the first order?"), transitions={ 'spoken' :'HEAR_WHICH_ORDER'})
smach.StateMachine.add('HEAR_WHICH_ORDER', HearWhichTable(robot),
transitions={ 'no_result' :'SAY_WHICH_ORDER', 'one' : 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_1', 'two': 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_2', 'three' : "FIRST_SAY_TAKE_ORDER_FROM_TABLE_3"})
# ############## first table ##############
for i, name in tables.iteritems():
next_i = i+1
if next_i > 3:
next_i = 1
smach.StateMachine.add('FIRST_SAY_TAKE_ORDER_FROM_TABLE_%d'%i, states.Say(robot, "Okay, I will take an order from table %d"%i, block=False),
transitions={ 'spoken' :'FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i})
smach.StateMachine.add('FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i, states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=name), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'FIRST_ASK_ORDER_TABLE_%d'%i, 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i})
smach.StateMachine.add('FIRST_ASK_ORDER_TABLE_%d'%i, AskOrder(robot, name),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'orders_done' : 'SAY_ORDERS_DONE'})
# ############## Loop over the reset of the tables until we have a beverage and a combo ##############
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_1', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="one"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_1', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_1', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_1'})
smach.StateMachine.add('HEAD_DOWN_TABLE_1', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_1'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_1', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_2', 'yes':'ASK_ORDER_TABLE_1','no':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('ASK_ORDER_TABLE_1', AskOrder(robot, "one"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="two"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_2', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_2', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_2'})
smach.StateMachine.add('HEAD_DOWN_TABLE_2', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_2'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_2', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_3', 'yes':'ASK_ORDER_TABLE_2','no':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('ASK_ORDER_TABLE_2', AskOrder(robot, "two"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_3', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="three"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_3', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_3', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_3'})
smach.StateMachine.add('HEAD_DOWN_TABLE_3', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_3'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_3', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_1', 'yes':'ASK_ORDER_TABLE_3','no':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('ASK_ORDER_TABLE_3', AskOrder(robot, "three"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('SAY_ORDERS_DONE', states.Say(robot, "I received enough orders for now, going back to the kitchen!", block=False),
transitions={ 'spoken' :'NAVIGATE_BACK_TO_THE_KITCHEN'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.2),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.4),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'SPEAK_ORDERS', 'goal_not_defined':'SPEAK_ORDERS'})
smach.StateMachine.add('SPEAK_ORDERS', SpeakOrders(robot),
transitions={ 'spoken' :'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('STORE_BEVERAGE_SIDE', StoreBeverageSide(robot),
transitions={ 'done' : 'NAVIGATE_TO_BEVERAGES'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'NAVIGATE_TO_BEVERAGES_BACKUP', 'goal_not_defined':'NAVIGATE_TO_BEVERAGES_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'STORE_BEVERAGE_SIDE', 'goal_not_defined':'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('SPEAK_I_SEE_THE_BEVERAGES', states.Say(robot, "The beverages are in front of me", block=False),
transitions={ 'spoken' :'DELIVER_BEVERAGE'})
smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"),
transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'NAVIGATE_TO_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_TO_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'})
smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"),
transitions={'succeeded':'NAVIGATE_BACK_TO_THE_KITCHEN_2', 'failed':'NAVIGATE_BACK_TO_THE_KITCHEN_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_DONE_WITH_CHALLENGE', 'unreachable':'SAY_DONE_WITH_CHALLENGE', 'goal_not_defined':'SAY_DONE_WITH_CHALLENGE'})
smach.StateMachine.add('SAY_DONE_WITH_CHALLENGE', states.Say(robot, "I'm done with this challenge and you are the banana king!"), transitions={ 'spoken' :'done'})
analyse_designators(sm, "restaurant")
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
use_entry_points=True),
transitions={
"Done": "SAY_GOTO_TARGET2",
"Aborted": "SAY_GOTO_TARGET2",
"Failed": "SAY_GOTO_TARGET2"
})
smach.StateMachine.add(
'SAY_GOTO_TARGET1',
states.Say(robot, [
"I will go to target 1 now", "I will now go to target 1",
"Lets go to target 1", "Going to target 1"
],
block=False),
transitions={'spoken': 'GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 1
#
######################################################################################################################################################
smach.StateMachine.add('GOTO_TARGET1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius1),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'RESET_ED_TARGET1',
'goal_not_defined': 'RESET_ED_TARGET1'
})
smach.StateMachine.add(
'SAY_TARGET1_REACHED',
states.Say(robot, [
"Reached target 1", "I have arrived at target 1",
"I am now at target 1"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
smach.StateMachine.add('RESET_ED_TARGET1',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET1_BACKUP'})
smach.StateMachine.add('GOTO_TARGET1_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius2),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'TIMEOUT1',
'goal_not_defined': 'TIMEOUT1'
})
smach.StateMachine.add('TIMEOUT1',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET1',
'time_out': 'SAY_TARGET1_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET1_FAILED',
states.Say(robot, [
"I am not able to reach target 1", "I cannot reach target 1",
"Target 1 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
######################################################################################################################################################
#
# TARGET 2
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET2',
states.Say(robot, [
"I will go to target 2 now", "I will now go to target 2",
"Lets go to target 2", "Going to target 2"
],
block=False),
transitions={'spoken': 'GOTO_TARGET2_PRE'})
smach.StateMachine.add(
'GOTO_TARGET2_PRE',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.target2_pre),
challenge_knowledge.target2_pre_radius1,
EntityByIdDesignator(robot, id=challenge_knowledge.target2)),
transitions={
'arrived': 'GOTO_TARGET2',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('GOTO_TARGET2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'DETERMINE_OBJECT',
'goal_not_defined': 'DETERMINE_OBJECT'
})
smach.StateMachine.add(
'DETERMINE_OBJECT',
DetermineObject(robot, challenge_knowledge.target2,
challenge_knowledge.target2_obstacle_radius),
transitions={
'done': 'GOTO_TARGET2_AGAIN',
'timeout': 'GOTO_TARGET2_AGAIN'
})
smach.StateMachine.add('GOTO_TARGET2_AGAIN',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'RESET_ED_TARGET2',
'goal_not_defined': 'RESET_ED_TARGET2'
})
smach.StateMachine.add(
'SAY_TARGET2_REACHED',
states.Say(robot, [
"Reached target 2", "I have arrived at target 2",
"I am now at target 2"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
smach.StateMachine.add('RESET_ED_TARGET2',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET2_BACKUP'})
smach.StateMachine.add('GOTO_TARGET2_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius2),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('TIMEOUT2',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET2_PRE',
'time_out': 'SAY_TARGET2_FAILED'
})
smach.StateMachine.add(
'SAY_TARGET2_FAILED',
states.Say(robot, [
"I am unable to reach target 2", "I cannot reach target 2",
"Target 2 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET3',
states.Say(robot, [
"I will go to target 3 now", "I will now go to target 3",
"Lets go to target 3", "Going to target 3"
],
block=False),
transitions={'spoken': 'GOTO_TARGET3'})
smach.StateMachine.add('GOTO_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius1),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'RESET_ED_TARGET3',
'goal_not_defined': 'RESET_ED_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_REACHED',
states.Say(robot, [
"Reached target 3", "I have arrived at target 3",
"I am now at target 3"
],
block=True),
transitions={'spoken': 'TURN'})
smach.StateMachine.add('RESET_ED_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius2),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'TIMEOUT3',
'goal_not_defined': 'TIMEOUT3'
})
smach.StateMachine.add('TIMEOUT3',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET3',
'time_out': 'SAY_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3", "I cannot reach target 3",
"Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'TURN'})
######################################################################################################################################################
#
# Follow waiter
#
######################################################################################################################################################
smach.StateMachine.add('TURN',
Turn(robot, challenge_knowledge.rotation),
transitions={'turned': 'SAY_STAND_IN_FRONT'})
smach.StateMachine.add(
'SAY_STAND_IN_FRONT',
states.Say(robot,
"Please stand in front of me!",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'})
# TODO: Fix concurrence
door_id_designator = VariableDesignator(
challenge_knowledge.target_door_1)
open_door_wp1_des = VariableDesignator(resolve_type=str)
open_door_wp2_des = VariableDesignator(resolve_type=str)
cc = smach.Concurrence(
['stopped', 'no_operator', 'lost_operator'],
default_outcome='no_operator',
child_termination_cb=lambda so: True,
outcome_map={
'stopped': {
'FOLLOW_OPERATOR': 'stopped'
},
# 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'},
'no_operator': {
'FOLLOW_OPERATOR': 'no_operator'
},
# 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'},
# 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'},
'lost_operator': {
'FOLLOW_OPERATOR': 'lost_operator'
}
})
with cc:
smach.Concurrence.add('FOLLOW_OPERATOR',
states.FollowOperator(robot, replan=True))
smach.Concurrence.add('DETERMINE_DOOR',
DetermineDoor(robot, door_id_designator))
smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK',
cc,
transitions={
'no_operator': 'FOLLOW_WITH_DOOR_CHECK',
'stopped': 'SAY_SHOULD_I_RETURN',
'lost_operator': 'SAY_SHOULD_I_RETURN'
})
# smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'})
smach.StateMachine.add('SAY_SHOULD_I_RETURN',
states.Say(robot,
"Should I return to target 3?",
look_at_standing_person=True),
transitions={'spoken': 'HEAR_SHOULD_I_RETURN'})
smach.StateMachine.add('HEAR_SHOULD_I_RETURN',
states.HearOptions(robot, ["yes", "no"]),
transitions={
'no_result': 'SAY_STAND_IN_FRONT',
"yes": "SELECT_WAYPOINTS",
"no": "SAY_STAND_IN_FRONT"
})
smach.StateMachine.add('SELECT_WAYPOINTS',
SelectWaypoints(door_id_designator,
open_door_wp1_des,
open_door_wp2_des),
transitions={'done': 'SAY_GOBACK_ARENA'})
######################################################################################################################################################
#
# RETURN TO ARENA DOOR
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOBACK_ARENA',
states.Say(robot, [
"I will go back to the arena", "I will return to the arena",
"Lets return to the arena", "Going back to the arena",
"Returning to the arena"
],
block=False),
transitions={'spoken': 'GOTO_ARENA_DOOR'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'RESET_ED_ARENA_DOOR',
'goal_not_defined': 'RESET_ED_ARENA_DOOR'
})
smach.StateMachine.add('ARENA_DOOR_REACHED',
states.Say(robot, [
"I am at the door of the arena",
"I have arrived at the door of the arena",
"I am now at the door of the arena"
],
block=True),
transitions={'spoken': 'SAY_OPEN_DOOR'})
smach.StateMachine.add('RESET_ED_ARENA_DOOR',
states.ResetED(robot),
transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR_BACKUP',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'TIMEOUT_ARENA_DOOR',
'goal_not_defined': 'TIMEOUT_ARENA_DOOR'
})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds_door),
transitions={
'not_yet': 'GOTO_ARENA_DOOR',
'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED'
})
smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED',
states.Say(robot, [
"I am unable to reach the arena door",
"I cannot reach the arena door",
"The arena door is unreachable"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# Opening Door
#
######################################################################################################################################################
smach.StateMachine.add(
'OPEN_DOOR',
states.OpenDoorByPushing(
robot,
EdEntityDesignator(robot, id_designator=open_door_wp1_des),
EdEntityDesignator(robot, id_designator=open_door_wp2_des)),
transitions={
'succeeded': 'SAY_RETURN_TARGET3',
'failed': 'TIMEOUT_ARENA_DOOR_OPENING'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR',
states.Say(robot, [
"I am going to open the door",
"Going to open the door of the arena", "Door, open sesame"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add(
'SAY_OPEN_DOOR_AGAIN',
states.Say(robot, [
"I failed to open the door. I will try it again",
"Let me try again to open the door"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'SAY_OPEN_DOOR_AGAIN',
'time_out': 'SAY_OPEN_DOOR_FAILED'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR_FAILED',
states.Say(robot, [
"I was not able to open the door. I am done with this challange",
"I was not able to open the door. I am done with this challange"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# RETURN TO TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_RETURN_TARGET3',
states.Say(robot, [
"I will go back to target 3 now", "I will return to target 3",
"Lets go to target 3 again", "Going to target 3, again"
],
block=False),
transitions={'spoken': 'RETURN_TARGET3'})
smach.StateMachine.add(
'RETURN_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius1),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'RESET_ED_RETURN_TARGET3',
'goal_not_defined': 'RESET_ED_RETURN_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_RETURN_REACHED',
states.Say(robot, [
"Reached target 3 again", "I have arrived at target 3 again",
"I am now at target 3 again"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
smach.StateMachine.add(
'RESET_ED_RETURN_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius2),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'TIMEOUT3_RETURN',
'goal_not_defined': 'TIMEOUT3_RETURN'
})
smach.StateMachine.add('TIMEOUT3_RETURN',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'RETURN_TARGET3',
'time_out': 'SAY_RETURN_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_RETURN_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3 again",
"I cannot reach target 3 again", "Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
######################################################################################################################################################
#
# TARGET EXIT
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_EXIT',
states.Say(robot, [
"I will move to the exit now. See you guys later!",
"I am done with this challenge. Going to the exit"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit1),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT',
'goal_not_defined': 'RESET_ED_EXIT'
})
smach.StateMachine.add('RESET_ED_EXIT',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit2),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT2',
'goal_not_defined': 'RESET_ED_EXIT2'
})
smach.StateMachine.add('RESET_ED_EXIT2',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP2'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit3),
radius=0.6),
transitions={
'arrived': 'GO_TO_EXIT_BACKUP3',
'unreachable': 'RESET_ED_EXIT3',
'goal_not_defined': 'RESET_ED_EXIT3'
})
smach.StateMachine.add('RESET_ED_EXIT3',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP3'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit4),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'AT_END',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "navigation")
return sm
def setup_statemachine(robot):
place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name")
place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot,
place_name,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
# For this challenge, unlocking is not needed
bag_arm_designator = empty_arm_designator.lockable()
bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
current_item = ds.VariableDesignator(Entity("dummy",
"dummy",
"/{}/base_link".format(robot.robot_name),
kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
None,
{},
[],
datetime.datetime.now()),
name="current_item")
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(robot, challenge_knowledge.starting_point),
transitions={'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'})
# TODO: learn operator state needs to be added before follow
# smach.StateMachine.add('WAIT_TO_FOLLOW',
# WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']),
# transitions={'success': 'FOLLOW_OPERATOR',
# 'abort': 'Aborted'})
smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER',
states.Say(robot, ["Are we at the car or should I follow you?"], block=True),
transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER',
WaitForOperatorCommand(robot,
possible_commands=[
"follow",
'follow me',
"here is the car",
"stop following",
"stop following me",
],
commands_as_outcomes=True),
transitions={'follow': 'FOLLOW_OPERATOR',
'follow me': 'FOLLOW_OPERATOR',
'here is the car': 'REMEMBER_CAR_LOCATION',
'stop following': 'REMEMBER_CAR_LOCATION',
'stop following me': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER',
'lost_operator': 'ASK_FOLLOW_OR_REMEMBER',
'no_operator': 'ASK_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
StoreCarWaypoint(robot),
transitions={'success': 'ASK_DESTINATION',
'abort': 'Aborted'})
smach.StateMachine.add('ASK_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"], block=True),
transitions={'spoken': 'WAIT_FOR_DESTINATION'})
smach.StateMachine.add('WAIT_FOR_DESTINATION',
WaitForOperatorCommand(robot,
possible_commands=challenge_knowledge.waypoints.keys(),
commands_as_userdata=True),
transitions={'success': 'GRAB_ITEM',
'abort': 'Aborted'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add('GRAB_ITEM',
GrabItem(robot, bag_arm_designator, current_item),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes.
'failed': 'BACKUP_CLOSE_GRIPPER'},
remapping={'target_room_in': 'command_recognized',
'target_room_out': 'target_room'})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'),
transitions={'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add('SAY_GOING_TO_ROOM',
states.Say(robot, ["Let me bring in your groceries",
"Helping you carry stuff",
"I'm going back inside"],
block=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add('GOTO_DESTINATION',
NavigateToRoom(robot),
transitions={'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
DropBagOnGround(robot, bag_arm_designator),
transitions={'succeeded': 'ASKING_FOR_HELP',
'failed': 'ASKING_FOR_HELP'})
smach.StateMachine.add('ASKING_FOR_HELP',
#TODO: look and then face new operator
states.Say(robot, "Please follow me and help me carry groceries into the house"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('GOTO_CAR',
states.NavigateToWaypoint(robot,
ds.EntityByIdDesignator(robot,
id=challenge_knowledge.waypoint_car['id']),
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'})
smach.StateMachine.add('OPEN_DOOR',
#TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('AT_END',
states.Say(robot, ["We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"],
block=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "help_me_carry")
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
with sm:
smach.StateMachine.add("INITIALIZE",
robot_smach_states.Initialize(robot),
transitions={
"initialized": "SAY_WAITING_FOR_TRIGGER",
"abort": "Aborted"
})
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
robot_smach_states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
door=False),
transitions={
"Done": "SAY_WAITING_FOR_TRIGGER",
"Failed": "Aborted",
"Aborted": "Aborted"
})
smach.StateMachine.add(
'SAY_WAITING_FOR_TRIGGER',
robot_smach_states.Say(robot, [
"Trigger me if you need me!", "Waiting for trigger",
"Waiting for you to call me!"
],
block=False),
transitions={"spoken": "WAIT_FOR_TRIGGER"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(
robot, ["gpsr"], "/amigo/trigger"),
transitions={
"gpsr": "VERIFY",
"preempted": "VERIFY"
})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "NAVIGATE_TO_ASK_WAYPOINT",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"})
smach.StateMachine.add("NAVIGATE_TO_ASK_WAYPOINT",
robot_smach_states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.ask_waypoint),
radius=0.3),
transitions={
'arrived':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'unreachable':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'goal_not_defined':
'DETERMINE_WHAT_TO_CLEAN_INSPECT'
})
smach.StateMachine.add(
"DETERMINE_WHAT_TO_CLEAN_INSPECT",
DetermineWhatToCleanInspect(robot),
transitions={
place["entity_id"]: "CLEAN_INSPECT_%s" % place["entity_id"]
for place in challenge_knowledge.inspection_places
})
for place in challenge_knowledge.inspection_places:
smach.StateMachine.add(
"CLEAN_INSPECT_%s" % place["entity_id"],
CleanInspect(robot, place["entity_id"], place["room_id"],
place["navigate_area"], place["segment_areas"]),
transitions={"done": "NAVIGATE_TO_ASK_WAYPOINT"})
return sm
def __init__(self, robot, room_id=ROOM_ID):
"""
Finds the people in the living room and takes pictures of them. Put then in a data struct and put this in
output keys.
Output keys:
* detected_people: same datastruct as was used in find my mates. Ask Rein for a pickled example
:param robot: (Robot) api object
:param room_id: (str) identifies the room in which the people are detected
"""
smach.StateMachine.__init__(self, outcomes=["done"], output_keys=["detected_people"])
with self:
# Move to start location
smach.StateMachine.add("NAVIGATE_TO_START",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(robot, WAYPOINT_ID), radius=0.3),
transitions={"arrived": "DETECT_PEOPLE",
"unreachable": "DETECT_PEOPLE",
"goal_not_defined": "DETECT_PEOPLE"})
@smach.cb_interface(outcomes=["done"], output_keys=["raw_detections"])
def detect_people(user_data):
person_detections = []
#look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide
look_angles = np.linspace(-np.pi / 4, np.pi / 4, 4) # From -pi/2 to +pi/2 to scan 180 degrees wide
head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle),
y=100 * math.sin(angle),
z=1.5,
frame_id="/%s/base_link" % robot.robot_name)
for angle in look_angles]
sentences = deque([
"Hi there mates, where are you, please look at me!",
"I am looking for my mates! Dippi dee doo! Pew pew!",
"You are all looking great today! Keep looking at my camera.",
"I like it when everybody is staring at me!"
])
look_at_me_sentences = deque([
"Please look at me",
"Let me check you out",
"Your eyes are beautiful",
"Try to look pretty, your face will pop up on the screen later!"
])
for _ in range(NUM_LOOKS):
sentences.rotate(1)
robot.speech.speak(sentences[0], block=False)
for head_goal in head_goals:
look_at_me_sentences.rotate(1)
robot.speech.speak(look_at_me_sentences[0], block=False)
robot.head.look_at_point(head_goal)
robot.head.wait_for_motion_done()
now = time.time()
rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
if rgb:
try:
persons = robot.perception.detect_person_3d(rgb, depth, depth_info)
except Exception as e:
rospy.logerr(e)
rospy.sleep(2.0)
else:
for person in persons:
if person.face.roi.width > 0 and person.face.roi.height > 0:
try:
person_detections.append({
"map_ps": robot.tf_listener.transformPoint("map", PointStamped(
header=rgb.header,
point=person.position
)),
"person_detection": person,
"rgb": rgb
})
except Exception as e:
rospy.logerr(
"Failed to transform valid person detection to map frame: {}".format(e))
rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now,
len(person_detections))
rospy.loginfo("Detected %d persons", len(person_detections))
user_data.raw_detections = person_detections
return 'done'
smach.StateMachine.add('DETECT_PEOPLE',
smach.CBState(detect_people),
transitions={"done": "FILTER_AND_CLUSTER"})
# Filter and cluster images
@smach.cb_interface(
outcomes=["done", "failed"],
input_keys=["raw_detections"],
output_keys=["detected_people"])
def filter_and_cluster_images(user_data):
"""
Filters the raw detections so that only people within the room maintain. Next, it clusters the images
so that only one image per person remains. This is stored in the user data
:param user_data: (smach.UserData)
:return: (str) Done
"""
try:
user_data.detected_people = _filter_and_cluster_images(
robot, user_data.raw_detections, room_id)
return "done"
except:
return "failed"
smach.StateMachine.add('FILTER_AND_CLUSTER',
smach.CBState(filter_and_cluster_images),
transitions={"done": "done",
"failed": "done"}) # ToDo: fallback
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_ARM_POSITIONS',
'abort': 'aborted'
})
smach.StateMachine.add('SET_ARM_POSITIONS',
SetPlateCarryingPose(robot),
transitions={'done': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('WAIT_FOR_KEY_PRESS',
WaitForKeyPress(),
transitions={
'1': 'SAY_IK_KOM',
'2': 'SAY_ALSJEBLIEFT',
'3': 'SAY_ALTIJD',
'4': 'SAY_STEKKER',
'5': 'SAY_OEPS'
})
smach.StateMachine.add('SAY_IK_KOM',
states.Say(robot,
"Ik kom eraan!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('SAY_ALSJEBLIEFT',
states.Say(robot,
"Alsjeblieft Rick. De enveloppen.",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add('SAY_ALTIJD',
states.Say(robot,
"Voor jou altijd, Rick.",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add(
'SAY_STEKKER',
states.Say(
robot,
"Ik heb stiekem gekeken, Rick. Maar als ik dat verklap, trekken ze de stekker eruit!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
smach.StateMachine.add(
'RESET_HEAD',
HeadCancel(robot),
transitions={'done': 'GO_BACK_TO_STARTING_POINT'})
smach.StateMachine.add('GO_BACK_TO_STARTING_POINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="starting_point"),
radius=knowledge.back_radius,
speak=False),
transitions={
'arrived': 'done',
'unreachable': 'WAIT_FOR_KEY_PRESS',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add('SAY_OEPS',
states.Say(robot,
"Oeps!",
look_at_standing_person=True,
language='nl',
voice='marjolijn',
block=False),
transitions={'spoken': 'WAIT_FOR_KEY_PRESS'})
return sm
def __init__(self, robot, assume_john):
"""
:param robot:
:param assume_john: bool indicating that John (the homeowner) is already there.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
guest_entity_des = ds.VariableDesignator(resolve_type=Entity,
name='guest_entity')
guest_name_des = ds.VariableDesignator('guest 1', name='guest_name')
guest_drink_des = ds.VariableDesignator(resolve_type=HMIResult,
name='guest_drink')
guest_drinkname_des = ds.FieldOfHMIResult(guest_drink_des,
semantics_field='drink',
name='guest_drinkname')
with self:
smach.StateMachine.add('LEARN_GUEST',
LearnGuest(robot, door_waypoint,
guest_entity_des, guest_name_des,
guest_drink_des),
transitions={
'succeeded': 'SAY_GOTO_OPERATOR',
'aborted': 'SAY_GOTO_OPERATOR',
'failed': 'SAY_GOTO_OPERATOR'
})
smach.StateMachine.add(
'SAY_GOTO_OPERATOR',
states.SayFormatted(robot, [
"Okidoki, you are {name} and you like {drink}, lets go inside. Please follow me"
],
name=guest_name_des,
drink=guest_drinkname_des,
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_LIVINGROOM'})
smach.StateMachine.add(
'GOTO_LIVINGROOM',
states.NavigateToWaypoint(
robot, livingroom_waypoint,
challenge_knowledge.waypoint_livingroom['radius']),
transitions={
'arrived': 'INTRODUCE_GUEST',
'unreachable': 'INTRODUCE_GUEST',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add('INTRODUCE_GUEST',
IntroduceGuest(robot,
guest_entity_des,
guest_name_des,
guest_drinkname_des,
assume_john=assume_john),
transitions={
'succeeded': 'FIND_SEAT_FOR_GUEST',
'abort': 'FIND_SEAT_FOR_GUEST'
})
smach.StateMachine.add(
'FIND_SEAT_FOR_GUEST',
FindEmptySeat(robot,
seats_to_inspect=challenge_knowledge.seats,
room=ds.EntityByIdDesignator(
robot, challenge_knowledge.sitting_room),
seat_is_for=guest_name_des),
transitions={
'succeeded': 'succeeded',
'failed': 'aborted'
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={
'stopped': 'ASK_FOR_TASK',
'lost_operator': 'ASK_FOR_TASK',
'no_operator': 'ASK_FOR_TASK'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add(
'WAIT_FOR_TASK',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
states.HandoverFromHuman(
robot,
self.bag_arm_designator,
"current_item",
self.current_item,
arm_configuration=challenge_knowledge.carrying_bag_pose),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER',
# For now in simulation timeout is considered a success.
'failed': 'BACKUP_CLOSE_GRIPPER'
})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(
robot,
self.bag_arm_designator,
gripperstate=GripperState.CLOSE),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'
})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(
robot, self.bag_arm_designator,
challenge_knowledge.driving_bag_pose),
transitions={
'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'
})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
hmc_states.DropBagOnGround(
robot, self.bag_arm_designator,
challenge_knowledge.drop_bag_pose),
transitions={'done': 'ASKING_FOR_HELP'})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, door_waypoint, guest_ent_des, guest_name_des,
guest_drink_des):
"""
Learn what a guest looks like and what his/her favourite drink is
:param robot: Robot that should execute this state
:param door_waypoint: Entity-designator resolving to a waypoint Where are guests expected to come in
:param guest_ent_des: Entity of the guest
:param guest_name_des: designator that the name (str) of the guest is written to
:param guest_drink_des: designator that the drink type (str) of the drink the guest wants
"""
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failed', 'aborted'])
self.drink_spec_des = ds.Designator(
challenge_knowledge.common.drink_spec, name='drink_spec')
with self:
smach.StateMachine.add(
'GOTO_DOOR',
states.NavigateToWaypoint(
robot, door_waypoint,
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'SAY_OPEN_DOOR',
'unreachable': 'SAY_OPEN_DOOR',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR',
states.Say(
robot,
["Someone please open the door, I'm expecting guests"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'SAY_PLEASE_COME_IN'})
smach.StateMachine.add(
'SAY_PLEASE_COME_IN',
states.Say(robot, [
"Please come in, I'm waiting for someone to step in front of me"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GUEST'})
smach.StateMachine.add("WAIT_FOR_GUEST",
states.WaitForPersonInFront(
robot, attempts=30, sleep_interval=1),
transitions={
'success': 'SAY_HELLO',
'failed': 'SAY_PLEASE_COME_IN'
})
smach.StateMachine.add(
'SAY_HELLO',
states.Say(robot, ["Hi there, I'll learn your face now"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'ASK_GUEST_NAME'})
smach.StateMachine.add('ASK_GUEST_NAME',
states.AskPersonName(
robot, guest_name_des.writeable,
challenge_knowledge.common.names),
transitions={
'succeeded': 'LEARN_PERSON',
'failed': 'SAY_HELLO',
'timeout': 'SAY_HELLO'
})
smach.StateMachine.add('LEARN_PERSON',
states.LearnPerson(
robot, name_designator=guest_name_des),
transitions={
'succeeded': 'SAY_GUEST_LEARNED',
'failed': 'SAY_FAILED_LEARNING'
})
smach.StateMachine.add(
'SAY_FAILED_LEARNING',
states.Say(robot,
["Not sure if I remember you, but I'll do my best"],
block=False),
transitions={'spoken': 'SAY_DRINK_QUESTION'})
smach.StateMachine.add(
'SAY_GUEST_LEARNED',
states.Say(robot, ["Okidoki, now I know what you look like"],
block=True),
transitions={'spoken': 'SAY_DRINK_QUESTION'})
smach.StateMachine.add('SAY_DRINK_QUESTION',
states.Say(robot,
["What's your favorite drink?"],
block=True),
transitions={'spoken': 'HEAR_DRINK_ANSWER'})
smach.StateMachine.add('HEAR_DRINK_ANSWER',
states.HearOptionsExtra(
robot, self.drink_spec_des,
guest_drink_des.writeable),
transitions={
'heard': 'RESET_1',
'no_result': 'SAY_DRINK_QUESTION'
})
smach.StateMachine.add('RESET_1',
states.ResetArms(robot),
transitions={'done': 'succeeded'})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
caller_id = "customer"
caller_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=caller_id)
orders = {}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_KITCHEN',
'abort': 'STOP'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForCustomer(robot, caller_id,
kitchen_designator),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'aborted': 'STOP',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived': 'RECITE_ORDER',
'unreachable': 'RECITE_ORDER',
'goal_not_defined': 'RECITE_ORDER'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order,"
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Dear mister, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
with self:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
transitions={"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.5),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1),
radius=0.7),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'ANNOUNCEMENT',
'goal_not_defined': 'ANNOUNCEMENT'})
# Part I: Set a table
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.grasp_furniture_id1,
grasp_furniture_id2=knowledge.grasp_furniture_id2,
place_furniture_id=knowledge.place_furniture_id),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id,
place_furniture_id1=knowledge.grasp_furniture_id1,
place_furniture_id2=knowledge.grasp_furniture_id2),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(
robot,
self.target_destination,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
'preempted': 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
# smach.StateMachine.add('FOLLOW_OPERATOR',
# states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True),
# transitions={'stopped': 'ASK_FOR_TASK',
# 'lost_operator': 'ASK_FOR_TASK',
# 'no_operator': 'FOLLOW_OPERATOR'})
# Use NEW:
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator2(robot),
transitions={
'Done': 'ASK_FOR_TASK',
'Failed': 'ASK_FOR_TASK',
'Aborted': 'FOLLOW_OPERATOR'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add('WAIT_FOR_TASK',
states.HearOptions(robot, ['yes', 'no']),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'CONFIRM_CAR_LOCATION',
'no_result': 'ASK_FOR_TASK'
})
smach.StateMachine.add(
'CONFIRM_CAR_LOCATION',
states.Say(
robot,
["OK, I will remember this location as the car location."],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'REMEMBER_CAR_LOCATION'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
#
# smach.StateMachine.add('CONFIRM_DESTINATION',
# states.Say(robot, [
# "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)],
# block=True,
# look_at_standing_person=True),
# transitions={'spoken': 'GRAB_ITEM'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
# states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item",
# self.current_item,
# arm_configuration=challenge_knowledge.carrying_bag_pose),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'timeout': 'BACKUP_CLOSE_GRIPPER',
# # For now in simulation timeout is considered a success.
# 'failed': 'BACKUP_CLOSE_GRIPPER'})
states.Say(robot, [
"I can't pick up the groceries since I don't have arms. Please place them in my basket."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'})
smach.StateMachine.add('WAIT_FOR_GRAB_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'SAY_GOING_TO_ROOM',
'preempted': 'Aborted'
})
# smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
# states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'failed': 'ARM_DRIVING_POSE'})
#
# smach.StateMachine.add('ARM_DRIVING_POSE',
# states.ArmToJointConfig(robot, self.bag_arm_designator,
# challenge_knowledge.driving_bag_pose),
# transitions={'succeeded': 'SAY_GOING_TO_ROOM',
# 'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'TURN_180_TO_REPLAN',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'TURN_180_TO_REPLAN',
hmc_states.TurnToReplan(robot),
transitions={
'success': 'GOTO_DESTINATION_BACKUP',
'abort': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
#'goal_not_defined': 'Aborted'})
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add(
'PUTDOWN_ITEM',
# hmc_states.DropBagOnGround(robot, self.bag_arm_designator,
# challenge_knowledge.drop_bag_pose),
states.Say(robot, [
"I can't put the groceries down since I have no arms. Please take them from my basket and put it down."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'})
smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'ASKING_FOR_HELP',
'preempted': 'Aborted'
})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'})
# smach.StateMachine.add('LEARN_OPERATOR',
# hmc_states.LearnOperator(robot),
# transitions={'learned': 'GOTO_CAR',
# 'failed': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
customer_id = 'current_customer'
customer_designator = states.util.designators.VariableDesignator(
resolve_type=Entity, name=customer_id)
orders = []
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SAY_WAVING',
'abort': 'STOP'
})
smach.StateMachine.add(
'SAY_WAVING',
states.Say(robot,
"Mr. Barman, please make sure that the people wave "
"slowly and put their arm up high. Like is shown "
"on my screen",
block=True),
transitions={'spoken': 'SHOW_IMAGE'})
smach.StateMachine.add(
'SHOW_IMAGE',
states.ShowImageState(
robot, "~/ros/kinetic/system/src/challenge_restaurant/"
"images/waving.jpg",
seconds=10),
transitions={
'succeeded': 'STORE_KITCHEN',
'failed': 'STORE_KITCHEN'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
# smach.StateMachine.add('WAIT_FOR_CUSTOMER',
# WaitForCustomer(robot, caller_id, kitchen_designator),
# transitions={'succeeded': 'SAY_I_HAVE_SEEN',
# 'aborted': 'STOP'})
# Implement new find state to detect nearest waving person
smach.StateMachine.add(
'WAIT_FOR_CUSTOMER',
states.FindFirstPerson(robot,
customer_designator.writeable,
properties={'tags': ['LWave', 'RWave']},
strict=False,
nearest=True,
speak=True,
look_range=(-np.pi / 4, np.pi / 4),
look_steps=4,
search_timeout=600), # 10 minutes
transitions={
'found': 'SAY_I_HAVE_SEEN',
'failed': 'WAIT_FOR_CUSTOMER'
})
# No Asking
# smach.StateMachine.add('SAY_I_HAVE_SEEN',
# states.Say(robot, 'I have seen a waving person, I will take the order, I will be there shortly! Coming your way my amigo!'),
# transitions={"spoken": 'NAVIGATE_TO_CUSTOMER'})
# End No Asking
# Asking for confirmation
smach.StateMachine.add(
'SAY_I_HAVE_SEEN',
states.Say(
robot,
'I have seen a waving person, should I take the order? '
'Please say "{0} take the order" or "{0} wait"'.format(
robot.robot_name)),
transitions={"spoken": 'WAIT_FOR_START'})
smach.StateMachine.add('WAIT_FOR_START',
AskTakeTheOrder(robot),
transitions={
'yes': 'SAY_NAVIGATE_TO_CUSTOMER',
'wait': 'WAIT_FOR_CUSTOMER',
'timeout': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_CUSTOMER',
states.Say(
robot,
"I am at your service, I will be there shortly! Coming your way my amigo!",
block=True),
transitions={'spoken': 'NAVIGATE_TO_CUSTOMER'})
# End Asking for confirmation
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=0.8),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, lets try it another way"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(
robot=robot,
entity_designator=customer_designator,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?", block=False),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'CLEAR_ORDER'})
smach.StateMachine.add(
'CLEAR_ORDER',
ClearOrders(orders=orders),
transitions={'succeeded': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order, "
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Hi there handsome, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
