def main():
rospy.init_node('ask_name_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.name = "robocup"
sm.userdata.purgeAll = True
smach.StateMachine.add(
'dropfaces',
drop_faces(),
transitions={'succeeded': 'SaveFaceSM', 'aborted': 'aborted'})
smach.StateMachine.add(
'SaveFaceSM',
SaveFaceSM(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'saveface_test_introspection', sm, '/SAVEFACE_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('ask_name_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.name = "robocup"
sm.userdata.purgeAll = True
smach.StateMachine.add('dropfaces',
drop_faces(),
transitions={
'succeeded': 'SaveFaceSM',
'aborted': 'aborted'
})
smach.StateMachine.add('SaveFaceSM',
SaveFaceSM(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('saveface_test_introspection', sm,
'/SAVEFACE_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('drop_face_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# it prepare the name and the function for drope
smach.StateMachine.add('prepare_learn_face',
prepare_drop_faces(),
transitions={
'succeeded': 'learn_face',
'aborted': 'aborted',
'preempted': 'preempted'
})
# it call the drop_face state
smach.StateMachine.add('learn_face',
drop_faces(),
transitions={
'succeeded': 'dummy',
'aborted': 'aborted_info'
})
# it prints the standard error
smach.StateMachine.add('aborted_info',
drop_faces_error(),
transitions={
'succeeded': 'dummy',
'aborted': 'dummy'
})
#dummy_state
smach.StateMachine.add('dummy',
dummy_state(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('drop_faces_introspection', sm,
'/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('drop_face_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# it prepare the name and the function for drope
smach.StateMachine.add(
'prepare_learn_face',
prepare_drop_faces(),
transitions={'succeeded':'learn_face','aborted' : 'aborted','preempted':'preempted'})
# it call the drop_face state
smach.StateMachine.add(
'learn_face',
drop_faces(),
transitions={'succeeded': 'dummy','aborted' : 'aborted_info'})
# it prints the standard error
smach.StateMachine.add(
'aborted_info',
drop_faces_error(),
transitions={'succeeded': 'dummy', 'aborted':'dummy'})
#dummy_state
smach.StateMachine.add(
'dummy',
dummy_state(),
transitions={'succeeded': 'succeeded', 'aborted':'aborted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'drop_faces_introspection', sm, '/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
# Params for drop faces - always will be the same
self.userdata.name_database = "find_me_database"
self.userdata.purgeAll = True
# Init the state machine - Drop faces in the database
smach.StateMachine.add(
'init_database',
drop_faces(),
remapping={'name':'name_database'},
transitions={'succeeded': 'save_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# Save the data from the TC
# We need one state machine that listen the name and then learn the face
smach.StateMachine.add(
'save_face',
SaveFaceSM(),
transitions={'succeeded': 'prepare_location', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the info to go to the room
smach.StateMachine.add(
'prepare_location',
prepare_location(),
transitions={'succeeded': 'go_location', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi(),
transitions={'succeeded': 'search_faces', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go around the room, it the robot find the TC return success
smach.StateMachine.add(
'search_faces',
SearchFacesSM(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord(),
transitions={'succeeded': 'prepare_say_found', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say "I found you!"
smach.StateMachine.add(
'prepare_say_found',
prepare_say_found(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_found',
text_to_say(),
transitions={'succeeded': 'gesture_recognition', 'aborted': 'aborted'})
# Recognize the direction
smach.StateMachine.add(
'gesture_recognition',
DummyStateMachine(),
transitions={'succeeded': 'go_side', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the direction
smach.StateMachine.add(
'go_side',
DummyStateMachine(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})