def test_con2_goal_cb(userdata, goal): test_goal = ExampleGoal() test_goal.order = userdata.test_order return test_goal
def main(): rospy.init_node('example_action_smach_state_machine') #TEST CONCURRENCE STATES WITH ACTIONS-------------------------------------------------------------------------------------------------- test_con1 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', input_keys = ['test_order'], output_keys= ['test_output_order'], child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') # Test Concurrence with goal and result callbacks that grab data from # the userdata input key passed into the state machine with test_con1: def test_con1_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) userdata.test_output_order = 6 return 'succeeded' def test_con1_goal_cb(userdata, goal): test_goal = ExampleGoal() test_goal.order = userdata.test_order return test_goal smach.Concurrence.add('TestCon1', SimpleActionState('example_server', ExampleAction, input_keys=['test_order'], output_keys=['test_output_order'], goal_cb=test_con1_goal_cb, result_cb=test_con1_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) test_con2 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', input_keys = ['test_order'], child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') with test_con2: def test_con2_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) return 'succeeded' def test_con2_goal_cb(userdata, goal): test_goal = ExampleGoal() test_goal.order = userdata.test_order return test_goal smach.Concurrence.add('TestCon2', SimpleActionState('example_server', ExampleAction, input_keys=['test_order'], goal_cb=test_con2_goal_cb, result_cb=test_con2_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) test_con3 = smach.Concurrence(outcomes=['test_done'], default_outcome = 'test_done', child_termination_cb = lambda outcome_map : True, outcome_cb = lambda outcome_map : 'test_done') with test_con3: def test_con3_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence)) return 'succeeded' test_goal = ExampleGoal() test_goal.order = 7 smach.Concurrence.add('TestCon3', SimpleActionState('example_server', ExampleAction, goal=test_goal, result_cb=test_con3_result_cb)) smach.Concurrence.add('testTimeout',sleep(20)) #SETUP ITERATOR STATE MACHINE-------------------------------------------------------------------------- sm_it = smach.StateMachine(outcomes=['outcome6']) sm_it.userdata.nums = range(25, 88, 3) sm_it.userdata.even_nums = [] sm_it.userdata.odd_nums = [] with sm_it: tutorial_it = smach.Iterator(outcomes = ['succeeded','aborted'], input_keys = ['nums', 'even_nums', 'odd_nums'], it = lambda: range(0, len(sm_it.userdata.nums)), output_keys = ['even_nums', 'odd_nums'], it_label = 'index', exhausted_outcome = 'succeeded') with tutorial_it: container_sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted','continue'], input_keys = ['nums', 'index', 'even_nums', 'odd_nums'], output_keys = ['even_nums', 'odd_nums']) with container_sm: #test wether even or odd with a conditional state smach.StateMachine.add('EVEN_OR_ODD', smach_ros.ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, input_keys=['nums', 'index']), {'true':'ODD', 'false':'EVEN' }) #add even state using a callback state @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def even_cb(ud): rospy.sleep(1) ud.even_nums.append(ud.nums[ud.index]) return 'succeeded' smach.StateMachine.add('EVEN', smach.CBState(even_cb), {'succeeded':'continue'}) #add odd state using a callback state @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def odd_cb(ud): rospy.sleep(1) ud.odd_nums.append(ud.nums[ud.index]) return 'succeeded' smach.StateMachine.add('ODD', smach.CBState(odd_cb), {'succeeded':'continue'}) #close container_sm smach.Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['continue']) smach.StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'outcome6', 'aborted':'outcome6'}) #STATE MACHINE----------------------------------------------------------------------------------------- # Create a SMACH state machine sm_top = smach.StateMachine(outcomes=['outcome5']) # Create userdata for top level state machine sm_top.userdata.first_goal = 5 sm_top.userdata.pass_data = 0 # Open the top container with sm_top: # Add Concurrent States smach.StateMachine.add('TestCon1', test_con1, transitions={'test_done':'TestCon2'}, remapping={'test_order':'first_goal', 'test_output_order':'pass_data'}) smach.StateMachine.add('TestCon2', test_con2, transitions={'test_done':'TestCon3'}, remapping={'test_order':'pass_data'}) smach.StateMachine.add('TestCon3', test_con3, transitions={'test_done':'SUB'}) # Create and add a low tier statemachine sm_sub = smach.StateMachine(outcomes=['outcome4']) with sm_sub: smach.StateMachine.add('FOO', example_states.Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'}) smach.StateMachine.add('BAR', example_states.Bar(), transitions={'outcome2':'FOO'}) # Add the low tier statemachine smach.StateMachine.add('SUB', sm_sub, transitions={'outcome4':'TUT_IT'}) # Add the low tier statemachine smach.StateMachine.add('TUT_IT', sm_it, transitions={'outcome6':'TestCon1'}) # Execute SMACH plan sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT') sis.start() outcome = sm_top.execute() rospy.spin() sis.stop()