def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
self.userdata.crowd_data = {
"males": 3,
"men": 2,
"females": 5,
"women": 3,
"children": 3,
"boys": 1,
"girls": 2,
"adults": 5,
"elders": 1,
"crowd_size": 8
}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'RIDDLE_GAME',
'abort': 'Aborted'})
smach.StateMachine.add("RIDDLE_GAME",
HearAndAnswerQuestions(
robot,
grammar=knowledge.grammar,
knowledge=common_knowledge,
num_questions=3,
),
transitions={'done': 'Done'},
remapping={'crowd_data':'crowd_data'})
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 setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.Initialize(robot),
transitions={
'initialized': "SAY_1",
"abort": "Aborted"
})
smach.StateMachine.add('SAY_1',
states.Say(robot, "Please ask me question one"),
transitions={'spoken': 'QUESTION_1'})
smach.StateMachine.add('QUESTION_1',
HearQuestion(robot),
transitions={'answered': 'SAY_2'})
smach.StateMachine.add('SAY_2',
states.Say(robot, "Please ask me question two"),
transitions={'spoken': 'QUESTION_2'})
smach.StateMachine.add('QUESTION_2',
HearQuestion(robot),
transitions={'answered': 'SAY_3'})
smach.StateMachine.add('SAY_3',
states.Say(robot,
"Please ask me question three"),
transitions={'spoken': 'QUESTION_3'})
smach.StateMachine.add('QUESTION_3',
HearQuestion(robot),
transitions={'answered': 'SAY_4'})
smach.StateMachine.add('SAY_4',
states.Say(robot,
"Please ask me question four"),
transitions={'spoken': 'QUESTION_4'})
smach.StateMachine.add('QUESTION_4',
HearQuestion(robot),
transitions={'answered': 'SAY_5'})
smach.StateMachine.add('SAY_5',
states.Say(robot,
"Please ask me question five"),
transitions={'spoken': 'QUESTION_5'})
smach.StateMachine.add('QUESTION_5',
HearQuestion(robot),
transitions={'answered': 'AT_END'})
smach.StateMachine.add('AT_END',
states.Say(robot, "That was all folks!"),
transitions={'spoken': 'Done'})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'DETECT_CROWD',
'abort': 'Aborted'})
smach.StateMachine.add("DETECT_CROWD",
DetectCrowd(robot),
transitions={'succeeded': 'Done',
'failed': 'Aborted'})
def __init__(self, robot):
"""
Final challenge of RWC 2019 Sydney
:param robot: (Robot) api object
"""
smach.StateMachine.__init__(self, outcomes=["done"])
with self:
# smach.StateMachine.add("START_ROBUST",
# states.StartChallengeRobust(robot, "initial_pose"),
# transitions={"Done": "LASER_POINTING",
# "Aborted": "LASER_POINTING",
# "Failed": "LASER_POINTING"})
smach.StateMachine.add("INIT",
states.Initialize(robot),
transitions={
"initialized": "LASER_POINTING",
"abort": "LASER_POINTING"
})
smach.StateMachine.add("LASER_POINTING",
DriveAndSwordFight(robot),
transitions={"done": "FIND_PEOPLE"})
smach.StateMachine.add("FIND_PEOPLE",
FindPeople(robot),
transitions={"done": "GET_ORDERS"})
smach.StateMachine.add("GET_ORDERS",
GetOrders(robot),
transitions={"done": "GET_DRINKS"})
smach.StateMachine.add("GET_DRINKS",
GetDrinks(robot=robot),
transitions={
"done": "done",
"failed": "done"
})
smach.StateMachine.add("SHOW_PEOPLE_DETECTION",
LightSaber(robot),
transitions={"done": "done"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.userdata.crowd_data = {
"males": 1,
"men": 2,
"females": 3,
"women": 4,
"children": 5,
"boys": 6,
"girls": 7,
"adults": 8,
"elders": 9,
"crowd_size": 10
}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'HEAR_QUESTION',
'abort': 'Aborted'
})
smach.StateMachine.add("HEAR_QUESTION",
HearQuestion(robot),
transitions={
'answered': 'HEAR_QUESTION_2',
'not_answered': 'HEAR_QUESTION_2'
},
remapping={'crowd_data': 'crowd_data'})
smach.StateMachine.add("HEAR_QUESTION_2",
HearQuestion(robot),
transitions={
'answered': 'Done',
'not_answered': 'Done'
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self, robot):
# type: (Robot) -> str
"""
Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des')
room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des')
objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des')
unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm')
with self:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot=robot),
transitions={"initialized": "INITIAL_POSE",
"abort": "aborted"})
smach.StateMachine.add("INITIAL_POSE",
states.SetInitialPose(robot,
challenge_knowledge.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"})
# Inspect bar and store the list of available drinks
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot,
entityDes=bar_designator,
navigation_area="in_front_of",
objectIDsDes=objects_list_des),
transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019
"failed": "INSPECT_FALLBACK"})
smach.StateMachine.add("CHECK_INSPECT_RESULT",
CheckInspect(objects_list_des,
[ClassificationResult]),
transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK",
"false": "INSPECT_FALLBACK"})
smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK",
IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects,
classification_list_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des.writeable,
max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS),
transitions={"done": "NAVIGATE_TO_ROOM",
"failed": "INSPECT_FALLBACK"})
# Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot,
unavailable_drink_designator=unav_drink_des.writeable,
objects=common_knowledge.objects),
transitions={"succeeded": "RESET_ROBOT",
"failed": "RESET_ROBOT"})
smach.StateMachine.add("RESET_ROBOT",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "NAVIGATE_TO_ROOM",
'failed': "NAVIGATE_TO_ROOM"})
# Navigate to the predefined room
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "SAY_HI",
"unreachable": "SAY_HI",
"goal_not_defined": "aborted"})
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)),
transitions={"spoken": "SERVE_DRINK_1"})
# Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state
# is constructed every time
for idx in range(1, challenge_knowledge.NR_DRINKS + 1):
next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE"
smach.StateMachine.add("SERVE_DRINK_{}".format(idx),
ServeOneDrink(robot=robot,
bar_designator=bar_designator,
room_id=challenge_knowledge.room_id,
room_designator=room_designator,
objects_list_des=objects_list_des,
unav_drink_des=unav_drink_des,
name_options=common_knowledge.names,
objects=common_knowledge.objects),
transitions={"succeeded": next_state,
"failed": next_state,
"aborted": next_state})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, "My job here is done. Enjoy your day and see you next time"),
transitions={"spoken": "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)
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'
})
def setup_statemachine(robot):
#retract old facts
robot.reasoner.query(Compound("retractall", Compound("challenge", "X")))
robot.reasoner.query(Compound("retractall", Compound("goal", "U","V","W", "X", "Y", "Z")))
robot.reasoner.query(Compound("retractall", Compound("explored", "X")))
robot.reasoner.query(Compound("retractall", Compound("state", "X", "Y")))
robot.reasoner.query(Compound("retractall", Compound("current_exploration_target", "X")))
robot.reasoner.query(Compound("retractall", Compound("current_object", "X")))
robot.reasoner.query(Compound("retractall", Compound("visited", "X")))
robot.reasoner.query(Compound("retractall", Compound("unreachable", "X")))
robot.reasoner.query(Compound("retractall", Compound("disposed", "X")))
robot.reasoner.query(Compound("retractall", Compound("point_roi_tried", "X")))
robot.reasoner.query(Compound("retractall", Compound("tasks_done", "X")))
robot.reasoner.query(Compound("retractall", Compound("tasks_max", "X")))
robot.reasoner.query(Compound("retractall", Compound("tasks_failed", "X")))
#Load database
robot.reasoner.query(Compound("load_database","tue_knowledge",'prolog/locations.pl'))
robot.reasoner.query(Compound("load_database","tue_knowledge",'prolog/egpsr.pl'))
#Assert the current challenge.
robot.reasoner.query(Compound("assertz",Compound("challenge", "egpsr")))
robot.reasoner.query(Compound("assertz",Compound("tasks_done", "0.0")))
robot.reasoner.query(Compound("assertz",Compound("tasks_max", "20.0"))) # Define how many tasks you want to perform
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
######################################################
##################### INITIALIZE #####################
######################################################
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={ 'initialized':'INTRODUCE_SHORT', ###### IN CASE NEXT STATE IS NOT "GO_TO_DOOR" SOMETHING IS SKIPPED
'abort':'Aborted'})
######################################################
#################### DEMO Version ####################
######################################################
smach.StateMachine.add("INTRODUCE_SHORT",
states.Say(robot,"Hi! I will just wait here and wonder if I can do something for you", block=True),
transitions={'spoken':'ASK_ACTION'})
smach.StateMachine.add("DEMO_SENTENCE",
states.Say(robot,"Although I know what to do, for now I am just showing that I am able to understand you!", block=True),
transitions={'spoken':'ASK_ACTION'})
smach.StateMachine.add("ASK_ACTION",
Ask_action(robot),
transitions={'done':'DEMO_SENTENCE',
'no_action':'ASK_ACTION'})
return sm
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# -----------------------------------------------------------------
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INSTRUCT_WAIT_FOR_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INSTRUCT_WAIT_FOR_DOOR",
states.Say(robot, [
"Hi there, I will now wait until you remove the cup",
"I'm waiting for you to remove the cup"
],
block=False),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("WAIT_FOR_DOOR",
states.WaitForDoorOpen(robot, timeout=10),
transitions={
"closed": "DOOR_CLOSED",
"open": "AWAIT_START"
})
smach.StateMachine.add(
"DOOR_CLOSED",
states.Say(robot, [
"I am waiting for you to remove the cup",
"I'd start, if you remove the cup from my laser"
]),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': 'LEARN_OPERATOR_FACE',
'no_response': 'AWAIT_START'
})
smach.StateMachine.add('LEARN_OPERATOR_FACE',
LearnOperatorFace(robot),
transitions={
'succeeded':
'WAIT_FOR_OPERATOR_TO_JOIN',
'failed': 'LEARN_OPERATOR_FACE'
})
@smach.cb_interface(outcomes=['done'])
def wait_a_sec(userdata=None):
robot.speech.speak(
"I will wait for 10 seconds for you to join the crowd",
block=True)
start = rospy.Time.now()
stop = rospy.Duration(10) + start
last_spoken = start
while rospy.Time.now() < stop:
if (rospy.Time.now() - last_spoken).to_sec() > 1.0:
robot.speech.speak("%d" %
(stop - rospy.Time.now()).to_sec())
last_spoken = rospy.Time.now()
return 'done'
smach.StateMachine.add('WAIT_FOR_OPERATOR_TO_JOIN',
smach.CBState(wait_a_sec),
transitions={'done': 'FORCE_DRIVE'})
@smach.cb_interface(outcomes=['done'])
def force_drive(userdata=None):
vth = 0.5
th = 3.1415
robot.head.cancel_goal()
robot.base.force_drive(0, 0, vth, th / vth)
return 'done'
smach.StateMachine.add('FORCE_DRIVE',
smach.CBState(force_drive),
transitions={'done': 'DETECT'})
smach.StateMachine.add('DETECT',
Detect(robot),
transitions={
'succeeded': 'END_CHALLENGE',
'failed': 'DETECT'
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self,
outcomes=['Done', 'WAIT_FOR_CUSTOMER'])
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': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForClickedCustomer(robot, caller_id),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'failed': 'WAIT_FOR_CUSTOMER',
'aborted': 'WAIT_FOR_CUSTOMER',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'TAKE_ORDER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'WAIT_FOR_CUSTOMER',
'misunderstood': 'TAKE_ORDER'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
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': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('BRING_OBJECTS',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
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_KITCHEN',
'preempted': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('RETURN_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_DONE',
states.Say(robot,
"That's it for today, I'm done"),
transitions={'spoken': 'Done'})
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": "SAY_START_CHALLENGE",
"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(
'SAY_START_CHALLENGE',
robot_smach_states.Say(robot, [
"Starting R5COP Cooperative cleaning demonstrator",
"What a mess here, let's clean this room!",
"Let's see if I can find some garbage here",
"All I want to do is clean this mess up!"
],
block=False),
transitions={"spoken": "INSPECT_0"})
for i, place in enumerate(challenge_knowledge.inspection_places):
next_i = i + 1 if i + 1 < len(
challenge_knowledge.inspection_places) else 0
smach.StateMachine.add(
"INSPECT_%d" % i,
CleanInspect(robot, place["entity_id"], place["room_id"],
place["navigate_area"], place["segment_areas"],
challenge_knowledge.known_types),
transitions={"done": "INSPECT_%d" % next_i})
return sm
def setup_statemachine(robot):
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('GOTO_WAYPOINT_1',
states.NavigateToWaypoint(
robot, challenge_knowledge.starting_point),
transitions={
'done': 'WAIT_TO_BEGIN',
"preempted": 'Aborted',
'error': 'Aborted'
})
smach.StateMachine.add('WAIT_TO_BEGIN',
states.AskContinue(robot, rospy.Duration(60)),
transitions={
'continue': 'BEGIN_MOVEMENT',
'no_response': 'WAIT_TO_BEGIN'
})
go_sm = smach.Concurrence(outcomes=['succes', 'iterate', 'failed'],
default_outcome='iterate',
outcome_map={
'succes': {
'MOVE_ARM': 'succes',
'MOVE_BASE': 'succes',
'GRAB_OBJECT': 'succes'
}
},
child_termination_cb=term_cb)
with go_sm:
smach.Concurrence.add('MOVE_ARM', MoveArm(robot))
smach.Concurrence.add(
'MOVE_BASE',
MoveBase(robot, entity_designator_area_name_map,
entity_lookat_designator))
smach.Concurrence.add('GRAB_OBJECT', GrabObject(robot))
smach.StateMachine.add('GO_SM',
go_sm,
transitions={
'iterate': 'GO_SM',
'succes': 'PO_SM',
'failed': 'Aborted'
})
po_sm = smach.Concurrence(outcomes=['succes', 'iterate', 'failed'],
default_outcome='iterate',
outcome_map={
'succes': {
'MOVE_ARM': 'succes',
'MOVE_BASE': 'succes',
'RELEASE_OBJECT': 'succes'
}
},
child_termination_cb=term_cb)
with po_sm:
smach.Concurrence.add('MOVE_ARM', MoveArm(robot))
smach.Concurrence.add(
'MOVE_BASE',
MoveBase(robot, entity_designator_area_name_map,
entity_lookat_designator))
smach.Concurrence.add('RELEASE_OBJECT', ReleaseObject(robot))
smach.StateMachine.add('PO_SM',
po_sm,
transitions={
'iterate': 'PO_SM',
'succes': 'Done',
'failed': 'Aborted'
})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=["succeeded",
"failed"], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(1000),
it_label="index",
exhausted_outcome="succeeded")
with range_iterator:
smach.Iterator.set_contained_state(
"SINGLE_ITEM",
single_item,
loop_outcomes=["succeeded", "failed"])
smach.StateMachine.add("RANGE_ITERATOR", range_iterator, {
"succeeded": "AT_END",
"failed": "Aborted"
})
# End setup iterator
smach.StateMachine.add("AT_END",
states.Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
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'])
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 __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:
# Part I: Set a table
smach.StateMachine.add('ENTER_ROOM', # Enter the room
states.Initialize(robot),
transitions={'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'})
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.cupboard,
grasp_furniture_id3=knowledge.cupboard,
place_furniture_id=knowledge.table),
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.table,
place_furniture_id1=knowledge.cupboard,
place_furniture_id3=knowledge.cupboard),
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'])
# - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - -
@smach.cb_interface(outcomes=['spoken'])
def sayHelloCB(userdata=None):
printOk("sayHelloCB")
robot.speech.speak("Hello " + personNameDes.resolve() + "!",
block=False)
return 'spoken'
@smach.cb_interface(outcomes=['done'])
def setOutcomeStatusCB(userdata=None, resultDes=None, succeeded=None):
printOk("setOutcomeStatusCB")
if True == succeeded:
printOk("Container finished successfully")
resultDes.write(0)
else:
printWarning("Container finished unsuccessfully")
resultDes.write(1)
return 'done'
# def my_cb(ud, x=0, y, z):
# ud.xyz = ud.q + x + y + z
# return 'foo'
# ...
# with sm:
# ...
# StateMachine.add('MY_CB', CBState(my_cb,
# cb_args=[10],
# cb_kwargs={'z':2,'y':3}),
# {'foo':'OTHER_STATE'})
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INIT_WM",
states.InitializeWorldModel(robot),
transitions={'done': 'SELECT_NEXT_CONTAINER'})
smach.StateMachine.add('SELECT_NEXT_CONTAINER',
test_states.SelectNextContainer(
robot, containerResultDes),
transitions={
'go_to_enter_room':
'ENTER_ROOM_CONTAINER',
'go_to_wait_person':
'WAIT_PERSON_CONTAINER',
'go_to_pick_up':
'PICK_UP_CONTAINER',
'go_to_recognize_people':
'RECOGNIZE_PEOPLE_CONTAINER',
'go_to_search_people':
'SEARCH_PEOPLE_CONTAINER'
})
# 'go_to_end_challenge':'END_CHALLENGE'
smach.StateMachine.add(
'SET_OUTCOME_STATUS_SUCCEEDED',
smach.CBState(setOutcomeStatusCB,
cb_kwargs={
'resultDes': containerResultDes.writeable,
'succeeded': True
}),
transitions={'done': 'SELECT_NEXT_CONTAINER'})
smach.StateMachine.add(
'SET_OUTCOME_STATUS_FAILED',
smach.CBState(setOutcomeStatusCB,
cb_kwargs={
'resultDes': containerResultDes.writeable,
'succeeded': False
}),
transitions={'done': 'SELECT_NEXT_CONTAINER'})
# Navigation test
smach.StateMachine.add('ENTER_ROOM_CONTAINER',
EnterRoomContainer(robot),
transitions={
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED',
'container_failed':
'SET_OUTCOME_STATUS_FAILED'
})
# Human Interaction Test
smach.StateMachine.add('WAIT_PERSON_CONTAINER',
WaitPersonContainer(robot),
transitions={
'container_success':
'LEARN_NAME_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Speech Test
smach.StateMachine.add('LEARN_NAME_CONTAINER',
LearnNameContainer(robot, personNameDes),
transitions={
'container_failed':
'SET_OUTCOME_STATUS_FAILED',
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED'
})
# Face Learning Test
smach.StateMachine.add('LEARN_FACE_CONTAINER',
LearnFaceContainer(robot, personNameDes),
transitions={
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED',
'container_failed':
'SET_OUTCOME_STATUS_FAILED'
})
# Face Recognition Test
smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER',
RecognizePeopleContainer(robot),
transitions={
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED',
'container_failed':
'SET_OUTCOME_STATUS_FAILED'
})
# Manipulation Test
smach.StateMachine.add('PICK_UP_CONTAINER',
PickUpContainer(robot, objectsIDsDes),
transitions={
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED',
'container_failed':
'SET_OUTCOME_STATUS_FAILED'
})
# Face Recogniton Test
smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER',
SearchPeopleContainer(robot, personNameDes),
transitions={
'container_success':
'SET_OUTCOME_STATUS_SUCCEEDED',
'container_failed':
'SET_OUTCOME_STATUS_FAILED',
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(
key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p
- robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo(
"Closest workspace: grasp from '{grasp}' and place on '{place}'"
.format(
grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id,
frame_stamped=closest_workspace.
place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id,
frame_stamped=closest_workspace.
grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[
0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
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'])
start_waypoint = ds.EntityByIdDesignator(robot,
id="manipulation_init_pose",
name="start_waypoint")
placed_items = []
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add("INIT_WM",
InitializeWorldModel(robot),
transitions={'done': 'AWAIT_START'})
# smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
# states.Say(robot, ["Hi there, I will now wait until you remove the cup",
# "I'm waiting for you to remove the cup"], block=False),
# transitions={"spoken": "WAIT_FOR_DOOR"})
#
# smach.StateMachine.add("WAIT_FOR_DOOR",
# states.WaitForDoorOpen(robot, timeout=10),
# transitions={"closed": "DOOR_CLOSED",
# "open": "AWAIT_START"})
#
# smach.StateMachine.add("DOOR_CLOSED",
# states.Say(robot, ["I am waiting for you to remove the cup",
# "I'd start, if you remove the cup from my laser"]),
# transitions={"spoken": "WAIT_FOR_DOOR"})
if USE_SLAM:
drive_state = "RESET_ED_SLAM"
else:
drive_state = "NAV_TO_START"
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': drive_state,
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
if USE_SLAM:
# vth = 1.0
# smach.StateMachine.add("NAV_TO_FIT_POSE",
# ForceDrive(robot, 0, 0, vth, 3.14/vth),
# transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("RESET_ED_SLAM",
states.ResetED(robot),
transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("FIT_ENTITY",
FitEntity(robot, CABINET),
transitions={
'succeeded': 'NAV_TO_START',
'failed': 'SAY_FITTING_FAILED'
})
smach.StateMachine.add(
"SAY_FITTING_FAILED",
states.Say(robot, [
"Fitting the {0} failed, I will stop now.".format(CABINET)
],
mood="sad"),
transitions={'spoken': 'Aborted'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"}, cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'FORCE_ROTATE',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("FORCE_ROTATE",
ForceRotate(robot, 0.5, 2.0, 30.0),
transitions={
'done': "NAV_TO_START",
'timedout': "INSPECT_SHELVES"
})
# smach.StateMachine.add("RESET_ED",
# states.ResetED(robot),
# transitions={'done' :'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, OBJECT_SHELVES),
transitions={
'succeeded': 'EXPORT_PDF',
'nothing_found': 'EXPORT_PDF',
'failed': 'EXPORT_PDF'
})
@smach.cb_interface(outcomes=["exported"])
def export_to_pdf(userdata=None):
global DETECTED_OBJECTS_WITH_PROBS
entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS]
# Export images (Only best MAX_NUM_ENTITIES_IN_PDF)
# pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge")
return "exported"
smach.StateMachine.add('EXPORT_PDF',
smach.CBState(export_to_pdf),
transitions={'exported': 'RANGE_ITERATOR'})
# Begin setup iterator
range_iterator = smach.Iterator(
outcomes=['succeeded', 'failed'], #Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded'
) #The exhausted argument should be set to the preffered state machine outcome
with range_iterator:
single_item = ManipRecogSingleItem(
robot,
ds.VariableDesignator(placed_items, [Entity],
name="placed_items"))
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "manipulation")
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'])
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - -
@smach.cb_interface(outcomes=['spoken'])
def sayHelloCB(userdata=None):
printOk("sayHelloCB")
robot.speech.speak("Hello " + personNameDes.resolve() + "!",
block=False)
return 'spoken'
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INIT_WM",
states.InitializeWorldModel(robot),
transitions={'done': 'ENTER_ROOM_CONTAINER'})
# smach.StateMachine.add( 'SELECT_NEXT_CONTAINER',
# test_states.SelectNextContainer(robot, containerResultDes),
# transitions={ 'go_to_enter_room':'ENTER_ROOM_CONTAINER',
# 'go_to_wait_person':'WAIT_PERSON_CONTAINER',
# 'go_to_pick_up':'PICK_UP_CONTAINER',
# 'go_to_recognize_people':'RECOGNIZE_PEOPLE_CONTAINER',
# 'go_to_search_people':'SEARCH_PEOPLE_CONTAINER',
# 'go_to_end_challenge':'END_CHALLENGE'})
# Navigation test
smach.StateMachine.add('ENTER_ROOM_CONTAINER',
EnterRoomContainer(robot),
transitions={
'container_success':
'WAIT_PERSON_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Human Interaction Test
smach.StateMachine.add('WAIT_PERSON_CONTAINER',
WaitPersonContainer(robot),
transitions={
'container_success':
'LEARN_NAME_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Speech Test
smach.StateMachine.add('LEARN_NAME_CONTAINER',
LearnNameContainer(robot, personNameDes),
transitions={
'container_failed': 'SAY_HELLO',
'container_success': 'SAY_HELLO'
})
smach.StateMachine.add(
'SAY_HELLO',
smach.CBState(sayHelloCB),
transitions={'spoken': 'LEARN_FACE_CONTAINER'})
# Face Learning Test
smach.StateMachine.add('LEARN_FACE_CONTAINER',
LearnFaceContainer(robot, personNameDes),
transitions={
'container_success':
'RECOGNIZE_PEOPLE_CONTAINER',
'container_failed':
'RECOGNIZE_PEOPLE_CONTAINER'
})
# Face Recognition Test
smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER',
RecognizePeopleContainer(robot),
transitions={
'container_success':
'PICK_UP_CONTAINER',
'container_failed': 'PICK_UP_CONTAINER'
})
# Manipulation Test
smach.StateMachine.add('PICK_UP_CONTAINER',
PickUpContainer(robot, objectsIDsDes),
transitions={
'container_success':
'SEARCH_PEOPLE_CONTAINER',
'container_failed':
'SEARCH_PEOPLE_CONTAINER'
})
# Face Recogniton Test
smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER',
SearchPeopleContainer(robot, personNameDes),
transitions={
'container_success': 'END_CHALLENGE',
'container_failed': 'END_CHALLENGE'
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
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):
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table),
transitions={'done': 'NAV_TO_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(robot,
{cabinet: "in_front_of"},
cabinet),
transitions={'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot),
transitions={'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'})
smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(outcomes=['succeeded', 'failed'], # Outcomes of the iterator state
input_keys=[], output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff
smach.Iterator.set_contained_state('SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator,
{'succeeded': 'AT_END',
'failed': 'Aborted'})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
