sm_human_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('srs_human_sensing')
00004 import rospy
00005 import smach
00006 import smach_ros
00007 
00008 from human_sensing_states import *
00009 from srs_human_sensing.msg import *
00010 
00011 def main():
00012 
00013     rospy.init_node('smach_example_state_machine')
00014     sm = smach.StateMachine(outcomes=['succeeded', 'failed', 'preempted'],
00015                                 input_keys=['sm_input'],
00016                                 output_keys=['humans_pose'])
00017 
00018     test=human_sensing_sm_input()
00019     test.label='alex'
00020     p=Pose2D()
00021     p.x=5
00022     p.y=5
00023     p.theta=5
00024     test.pose_furniture.append(p)
00025     sm.userdata.sm_input=test
00026 
00027     with sm:
00028                
00029             smach.StateMachine.add('Leg Detection', leg_detection(),
00030                     transitions={'succeeded':'Move to better position', 'failed':'failed', 'preempted':'preempted','retry':'Leg Detection'})
00031             
00032             smach.StateMachine.add('Move to better position', move_to_better_position(),
00033                     transitions={'succeeded':'Face detection','failed':'failed', 'preempted':'preempted'},
00034                     remapping={'pose_list_out':'pose_list','humans_pose_out':'humans_pose',
00035                                'id_out':'id','person_label_out':'person_label'})
00036             smach.StateMachine.add('Face detection', face_detection(),
00037                     transitions={'succeeded':'Body detection','retry':'Move to better position', 'failed':'failed','preempted':'preempted'},
00038                     remapping={'pose_list_output':'pose_list','humans_pose_out':'humans_pose',
00039                                'id_out':'id'})
00040             smach.StateMachine.add('Body detection', body_detection(),
00041                     transitions={'succeeded':'compare_detections', 'failed':'failed','preempted':'preempted'},
00042                     remapping={'pose_list_output':'pose_list','humans_pose_out':'humans_pose',
00043                                'id_out':'id','face_list_out':'face_list'})
00044             smach.StateMachine.add('compare_detections', compare_detections(),
00045                     transitions={'succeeded':'succeeded','retry':'Move to better position', 'failed':'failed','preempted':'preempted'},
00046                     remapping={'pose_list_output':'pose_list','humans_pose_out':'humans_pose',
00047                                'id_out':'id'})
00048 
00049     outcome = sm.execute()
00050 
00051 
00052     sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
00053     sis.start()
00054 
00055 
00056     # Wait for ctrl-c to stop the application
00057     rospy.spin()
00058     sis.stop()
00059     
00060     
00061 if __name__ == '__main__':
00062     main()


srs_human_sensing
Author(s): Alex
autogenerated on Mon Oct 6 2014 09:19:16