def get_sub_machine(children_tags):
"""
returns a smach.State machine with len(children_tags) children. Only children whose tag is not "" are registered.
"""
node = smach.StateMachine(outcomes=['ok'])
with node:
for idx, tag in enumerate(children_tags):
register = tag != ""
name = tag
if not register:
name = "<Unnamed " + str(idx + 1) + ">"
leaf = Dummy(name)
if register:
ltm.register_state(leaf, [tag])
transition = 'ok'
if idx + 1 < len(children_tags):
transition = children_tags[idx + 1].upper()
if transition == "":
transition = ("<Unnamed " + str(idx + 2) + ">").upper()
smach.StateMachine.add(name.upper(),
leaf,
transitions={'ok': transition})
return node
def get_instance():
sm = smach.StateMachine(outcomes=['ok'])
ltm.register_state(sm, ["root"])
with sm:
state_1 = Dummy("A1")
state_2 = Dummy("A2")
state_3 = Looper("A3", times=3)
state_4 = Dummy("A4")
# register children
ltm.register_state(state_1, ["A1"])
ltm.register_state(state_2, ["A2"])
ltm.register_state(state_3, ["A3"])
ltm.register_state(state_4, ["A4"])
smach.StateMachine.add('A1', state_1, transitions={'ok': 'A2'})
smach.StateMachine.add('A2', state_2, transitions={'ok': 'A3'})
smach.StateMachine.add('A3',
state_3,
transitions={
'ok': 'A4',
'loop': 'A2'
})
smach.StateMachine.add('A4', state_4, transitions={'ok': 'ok'})
return sm
def get_instance():
sm = smach.StateMachine(outcomes=['succeeded'])
ltm.register_state(sm, ["crowd_analysis"])
with sm:
smach.StateMachine.add(
'INIT_TALK',
Talk("I want to play. Let's find some people first."),
transitions={'succeeded': 'LOOK_FOR_CROWD'})
smach.StateMachine.add('LOOK_FOR_CROWD',
get_look_for_crowd_sm(),
transitions={'succeeded': 'ANALYZE_CROWD'})
smach.StateMachine.add('ANALYZE_CROWD',
get_analyze_crowd_sm(),
transitions={
'succeeded': 'END_TALK',
'failed': 'LOOK_FOR_CROWD'
})
smach.StateMachine.add('END_TALK',
Talk("That was a lot of fun."),
transitions={'succeeded': 'succeeded'})
return sm
def build_goodbye_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['human'])
ltm.register_state(sm, ["hri_goodbye"])
with sm:
smach.StateMachine.add('SAY_GOODBYE',
SayHumanResults(robot),
transitions={'succeeded': 'succeeded'})
return sm
def build_wait_human_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
ltm.register_state(sm, ["approach_human"])
approach_sm = WaitForButton(robot, 30.0)
ltm.register_state(approach_sm, ["approach_human"])
with sm:
smach.StateMachine.add('APPROACH_TO_HUMAN',
approach_sm,
transitions={'succeeded': 'succeeded'})
return sm
def get_instance():
root = smach.StateMachine(outcomes=['ok'])
ltm.register_state(root, ["root"])
with root:
node_1 = get_sub_machine(['A1', 'A2', 'A3'])
node_2 = get_sub_machine(['B1', 'B2'])
ltm.register_state(node_2, ['node_2'])
smach.StateMachine.add('NODE_1', node_1, transitions={'ok': 'NODE_2'})
smach.StateMachine.add('NODE_2', node_2, transitions={'ok': 'ok'})
return root
def get_instance():
sm = smach.StateMachine(outcomes=['ok'])
with sm:
leaf_1 = Dummy("leaf_1")
leaf_2 = Dummy("leaf_2")
# register children
ltm.register_state(leaf_1, ["leaf_1"])
ltm.register_state(leaf_2, ["leaf_2"])
smach.StateMachine.add('LEAF_1', leaf_1, transitions={'ok': 'LEAF_2'})
smach.StateMachine.add('LEAF_2', leaf_2, transitions={'ok': 'ok'})
return sm
def get_analyze_crowd_sm():
sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
ltm.register_state(sm, ["crowd_analysis"])
with sm:
smach.StateMachine.add('ANALYZE_PEOPLE',
AnalyzePeople(),
transitions={
'succeeded': 'GIVE_PEOPLE_INFO',
'failed': 'failed'
})
smach.StateMachine.add('GIVE_PEOPLE_INFO',
Talk("You are all very ugly .. uhggg."),
transitions={'succeeded': 'succeeded'})
return sm
def get_instance():
sm = smach.StateMachine(outcomes=['succeeded'])
ltm.register_state(sm, ["robocup", "stage_1", "spr"])
with sm:
smach.StateMachine.add('CROWD_ANALYSIS',
crowd_analysis.get_instance(),
transitions={'succeeded': 'RIDDLE_GAME'})
smach.StateMachine.add('RIDDLE_GAME',
riddle_game.get_instance(),
transitions={'succeeded': 'BLIND_MAN_GAME'})
smach.StateMachine.add('BLIND_MAN_GAME',
blind_man_game.get_instance(),
transitions={'succeeded': 'succeeded'})
return sm
def get_instance():
root = smach.StateMachine(outcomes=['ok'])
ltm.register_state(root, ["root"])
grandpa = smach.StateMachine(outcomes=['ok'])
with grandpa:
father = get_sub_machine(['A1', 'A2', 'A3'])
smach.StateMachine.add('FATHER', father, transitions={'ok': 'ok'})
with root:
node_b = get_sub_machine(['B1', 'B2'])
ltm.register_state(node_b, ['node_b'])
smach.StateMachine.add('GRANDPA', grandpa, transitions={'ok': 'B'})
smach.StateMachine.add('B', node_b, transitions={'ok': 'ok'})
return root
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
ltm.register_state(sm, ["ltm_demo", "coffee_session"])
with sm:
smach.StateMachine.add('GET_ORDER',
build_get_order_sm(robot),
transitions={'succeeded': 'PREPARE_COFFEE'})
smach.StateMachine.add('PREPARE_COFFEE',
build_prepare_coffee_sm(robot),
transitions={'succeeded': 'GIVE_ORDER'})
smach.StateMachine.add('GIVE_ORDER',
build_give_order_sm(robot),
transitions={'succeeded': 'succeeded'})
return sm
def build_introduction_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['human'])
ltm.register_state(sm, ["hri_introduction"])
with sm:
smach.StateMachine.add('SAY_HELLO',
Speak(robot, text="Hello, i am bender ..."),
transitions={'succeeded': 'ASK_NAME'})
smach.StateMachine.add('ASK_NAME',
Speak(robot,
text="Nice to meet you " + Data.NAME),
transitions={'succeeded': 'RECORD_HUMAN'})
smach.StateMachine.add('RECORD_HUMAN',
RecordHuman(),
transitions={'succeeded': 'succeeded'})
return sm
def get_look_for_crowd_sm():
sm = smach.StateMachine(outcomes=['succeeded'])
ltm.register_state(sm, ["look_for_crowd"])
with sm:
smach.StateMachine.add(
'INIT_TALK',
Talk("Where are you?. I will find you sooner or later."),
transitions={'succeeded': 'LOOK_FOR_PEOPLE'})
smach.StateMachine.add('LOOK_FOR_PEOPLE',
LookForPeople(),
transitions={'succeeded': 'END_TALK'})
smach.StateMachine.add('END_TALK',
Talk("I found you."),
transitions={'succeeded': 'succeeded'})
return sm
def build_facial_features_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['human'],
output_keys=['human'])
sm.userdata.features = ['gender', 'age']
ltm.register_state(sm, ["facial_analysis"])
with sm:
smach.StateMachine.add('INIT',
Speak(robot,
text="Please, look me in the eyes ..."),
transitions={'succeeded': 'GET_AGE_AND_GENRE'})
age_gender_sm = WaitForButton(robot, 30.0)
ltm.register_state(age_gender_sm,
["age_estimation", "gender_estimation"])
smach.StateMachine.add(
'GET_AGE_AND_GENRE',
age_gender_sm,
transitions={'succeeded': 'RECORD_AGE_AND_GENRE'})
smach.StateMachine.add('RECORD_AGE_AND_GENRE',
RecordHumanAgeAndGenre(),
transitions={'succeeded': 'GET_EMOTION'})
emotion_sm = WaitForButton(robot, 30.0)
ltm.register_state(emotion_sm, ["emotion_estimation"])
smach.StateMachine.add('GET_EMOTION',
emotion_sm,
transitions={'succeeded': 'RECORD_EMOTION'})
smach.StateMachine.add('RECORD_EMOTION',
RecordHumanEmotion(),
transitions={'succeeded': 'succeeded'})
return sm
def build_get_order_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['human'])
ltm.register_state(sm, ["get_order"])
wait_face_sm = WaitForButton(robot)
approach_sm = WaitForButton(robot)
ltm.register_state(approach_sm, ["wait_face"])
ltm.register_state(approach_sm, ["approach_human"])
with sm:
smach.StateMachine.add('WAIT_FACE',
wait_face_sm,
transitions={'succeeded': 'APPROACH_TO_HUMAN'})
smach.StateMachine.add('APPROACH_TO_HUMAN',
approach_sm,
transitions={'succeeded': 'SAY_HELLO'})
smach.StateMachine.add('SAY_HELLO',
Speak(robot,
text="Hello, " + Data.NAME +
" i will prepare a coffee for you."),
transitions={'succeeded': 'RECORD_HUMAN'})
smach.StateMachine.add('RECORD_HUMAN',
RecordHuman(),
transitions={'succeeded': 'succeeded'})
return sm
def getInstance(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
ltm.register_state(sm, ["ltm_demo", "human_session"])
with sm:
smach.StateMachine.add('WAIT_HUMAN',
build_wait_human_sm(robot),
transitions={'succeeded': 'INTRODUCTION'})
smach.StateMachine.add(
'INTRODUCTION',
build_introduction_sm(robot),
transitions={'succeeded': 'ANALYZE_FACIAL_FEATURES'})
smach.StateMachine.add('ANALYZE_FACIAL_FEATURES',
build_facial_features_sm(robot),
transitions={'succeeded': 'GOODBYE'})
smach.StateMachine.add('GOODBYE',
build_goodbye_sm(robot),
transitions={'succeeded': 'succeeded'})
return sm
def build_prepare_coffee_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
ltm.register_state(sm, ["deliver_order"])
go_sm = WaitForButton(robot)
prepare_sm = WaitForButton(robot)
ltm.register_state(go_sm, ["go_to_kitchen"])
ltm.register_state(prepare_sm, ["prepare_coffee"])
with sm:
smach.StateMachine.add('GO_TO_KITCHEN',
go_sm,
transitions={'succeeded': 'PREPARE_COFFEE'})
smach.StateMachine.add('PREPARE_COFFEE',
prepare_sm,
transitions={'succeeded': 'succeeded'})
return sm
def build_give_order_sm(robot):
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['human'])
ltm.register_state(sm, ["deliver_order"])
approach_sm = WaitForButton(robot)
give_sm = DeliverCoffee(robot)
ltm.register_state(approach_sm, ["approach_human"])
ltm.register_state(give_sm, ["deliver_coffee"])
with sm:
smach.StateMachine.add('APPROACH_TO_HUMAN',
approach_sm,
transitions={'succeeded': 'GIVE_COFFEE'})
smach.StateMachine.add('GIVE_COFFEE',
give_sm,
transitions={'succeeded': 'RECORD_HUMAN'})
smach.StateMachine.add('RECORD_HUMAN',
RecordHappyHuman(),
transitions={'succeeded': 'succeeded'})
return sm
def get_instance():
sm = get_sub_machine(["A1", "A2", "", "A4"])
ltm.register_state(sm, ["root"])
return sm
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
ltm.register_state(self, ["look_for_crowd"])
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
ltm.register_state(self, ["analyze_crowd"])
self.failed = False