def construct_sm(): sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'], it=lambda: range(0, 10), it_label='index', input_keys=[], output_keys=[], exhausted_outcome='succeeded') with tutorial_it: container_sm = utils.TargetSelectorContainer('explore') #close container_sm Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['target_sent']) #close the tutorial_it StateMachine.add('TUTORIAL_IT', tutorial_it, { 'succeeded': 'succeeded', 'aborted': 'aborted' }) return sm
def __init__(self): rospy.init_node('speech_recognition') rospy.on_shutdown(self.shutdown) self.find_people = StateMachine( outcomes=['succeeded', 'aborted', 'error']) self.test_bool = False with self.find_people: self.answer = Iterator(outcomes=['succeeded', 'aborted', 'error'], input_keys=['people_condition'], output_keys=[], it=lambda: range(0, 5), exhausted_outcome='succeeded') with self.answer: Iterator.set_contained_state('ANSWER_STATE', Anwser(), loop_outcomes=['succeeded']) StateMachine.add('ANSWER', self.answer, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }) out = self.find_people.execute() if out == 'succeeded': self.test_bool = True rospy.logwarn('test finished, all thing is done well!') else: rospy.logerr('test failed ,something goes wrong')
def construct_sm(): sm = StateMachine(outcomes = ['succeeded','aborted','preempted']) sm.userdata.nums = range(25, 88, 3) sm.userdata.even_nums = [] sm.userdata.odd_nums = [] with sm: ## %Tag(ITERATOR)% tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = ['nums', 'even_nums', 'odd_nums'], it = lambda: range(0, len(sm.userdata.nums)), output_keys = ['even_nums', 'odd_nums'], it_label = 'index', exhausted_outcome = 'succeeded') ## %EndTag(ITERATOR)% ## %Tag(CONTAINER)% with tutorial_it: container_sm = 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 StateMachine.add('EVEN_OR_ODD', ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, input_keys=['nums', 'index']), {'true':'ODD', 'false':'EVEN' }) #add even state @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def even_cb(ud): ud.even_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('EVEN', CBState(even_cb), {'succeeded':'continue'}) #add odd state @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def odd_cb(ud): ud.odd_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('ODD', CBState(odd_cb), {'succeeded':'continue'}) ## %EndTag(CONTAINER)% ## %Tag(ADDCONTAINER)% #close container_sm Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['continue']) ## %EndTag(ADDCONTAINER)% ## %Tag(ADDITERATOR)% #close the tutorial_it StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'succeeded', 'aborted':'aborted'}) ## %EndTag(ADDITERATOR)% return sm
def construct_sm(): sm = StateMachine(outcomes = ['succeeded','aborted','preempted']) sm.userdata.nums = range(25, 88, 3) sm.userdata.even_nums = [] sm.userdata.odd_nums = [] with sm: ## %Tag(ITERATOR)% tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = ['nums', 'even_nums', 'odd_nums'], it = lambda: range(0, len(sm.userdata.nums)), output_keys = ['even_nums', 'odd_nums'], it_label = 'index', exhausted_outcome = 'succeeded') ## %EndTag(ITERATOR)% ## %Tag(CONTAINER)% with tutorial_it: container_sm = 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 StateMachine.add('EVEN_OR_ODD', ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, input_keys=['nums', 'index']), {'true':'ODD', 'false':'EVEN' }) #add even state @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def even_cb(ud): ud.even_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('EVEN', CBState(even_cb), {'succeeded':'continue'}) #add odd state @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def odd_cb(ud): ud.odd_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('ODD', CBState(odd_cb), {'succeeded':'continue'}) ## %EndTag(CONTAINER)% ## %Tag(ADDCONTAINER)% #close container_sm Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['continue']) ## %EndTag(ADDCONTAINER)% ## %Tag(ADDITERATOR)% #close the tutorial_it StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'succeeded', 'aborted':'aborted'}) ## %EndTag(ADDITERATOR)% return sm
def execute(self): sm_top = smach.StateMachine( outcomes=['succeeded', 'preemted', 'aborted', 'continue']) with sm_top: self.rooms_go = Iterator( outcomes=['succeeded', 'preemted', 'aborted'], input_keys=[], output_keys=['index'], it=lambda: range(0, 3), it_label='index', exhausted_outcome='succeeded') with self.rooms_go: self.rooms_go_proton = StateMachine( outcomes=['succeeded', 'aborted', 'continue'], input_keys=['index']) with self.rooms_go_proton: #self.current_pos = self.RoomsPoses[index] self.rooms_go_proton.userdata.current_poses = self.RoomsPoses self.rooms_go_proton.userdata.current_pos2 = 0 self.rooms_go_proton.add('INDEXTRANS', IndexTrans(), transitions={'succeeded': 'GO'}, remapping={ 'poses': 'current_poses', 'current_pos': 'current_pos2', 'index': 'index' }) self.rooms_go_proton.add( 'GO', NavStack(), transitions={ 'succeeded': 'continue', 'aborted': 'GO', 'error': 'continue' }, remapping={'pos_xm': 'current_pos2'}) Iterator.set_contained_state('CONTAINER_STATE', self.rooms_go_proton, loop_outcomes=['continue']) sm_top.add('ROOMS_GO', self.rooms_go, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }, remapping={'index': 'index'}) out = sm_top.execute() rospy.logerr(out)
def make_iterator(container, max_iter=1): it = Iterator(outcomes = ['preempted','aborted','time_out'], it = lambda: range(0, max_iter), it_label = 'index', input_keys=[], output_keys=[], exhausted_outcome = 'time_out') with it: Iterator.set_contained_state('TARGET_CONTROLLER', container, loop_outcomes=['target_sent']) return it
def make_iterator(container, max_iter=1): it = Iterator(outcomes=['preempted', 'aborted', 'time_out'], it=lambda: range(0, max_iter), it_label='index', input_keys=[], output_keys=[], exhausted_outcome='time_out') with it: Iterator.set_contained_state('TARGET_CONTROLLER', container, loop_outcomes=['target_sent']) return it
def __init__(self): Iterator.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['log_mission'], it=[], output_keys=['log_mission'], it_label='waypoint_id', exhausted_outcome='succeeded') self.register_start_cb(self.set_iterator_list) with self: self.sm_nav = StateMachine( outcomes=['succeeded', 'preempted', 'aborted', 'continue'], input_keys=['waypoint_id', 'log_mission'], output_keys=['log_mission']) self.sm_nav.register_start_cb(self.start_cb_nav) self.sm_nav.register_termination_cb(self.termination_cb_nav) with self.sm_nav: StateMachine.add('MOVE_BASE', SimpleActionState( 'move_base', MoveBaseAction, input_keys=['waypoint_id', 'log_mission'], goal_cb=self.move_base_goal_cb), transitions={ 'succeeded': 'SAVE_DATA', 'aborted': 'continue' }) self.save_data_service = ServiceState( 'make_data_acquisition', RequestSaveData, response_cb=self.save_data_response_cb) StateMachine.add('SAVE_DATA', self.save_data_service, transitions={'succeeded': 'continue'}) # Close the sm_nav machine and add it to the iterator Iterator.set_contained_state('FOLLOW_PATH', self.sm_nav, loop_outcomes=['continue'])
def construct_sm(): sm = StateMachine(outcomes = ['succeeded','aborted','preempted']) with sm: tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'], it = lambda: range(0, 10), it_label = 'index', input_keys=[], output_keys=[], exhausted_outcome = 'succeeded') with tutorial_it: container_sm = utils.TargetSelectorContainer('explore') #close container_sm Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['target_sent']) #close the tutorial_it StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'succeeded', 'aborted':'aborted'}) return sm
def construct_sm(): sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) sm.userdata.nums = range(25, 88, 3) sm.userdata.even_nums = [] sm.userdata.odd_nums = [] with sm: ## %Tag(ITERATOR)% tutorial_it = Iterator( outcomes=["succeeded", "preempted", "aborted"], input_keys=["nums", "even_nums", "odd_nums"], it=lambda: range(0, len(sm.userdata.nums)), output_keys=["even_nums", "odd_nums"], it_label="index", exhausted_outcome="succeeded", ) ## %EndTag(ITERATOR)% ## %Tag(CONTAINER)% with tutorial_it: container_sm = 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 StateMachine.add( "EVEN_OR_ODD", ConditionState( cond_cb=lambda ud: ud.nums[ud.index] % 2, input_keys=["nums", "index"], ), { "true": "ODD", "false": "EVEN" }, ) # add even state @smach.cb_interface( input_keys=["nums", "index", "even_nums"], output_keys=["odd_nums"], outcomes=["succeeded"], ) def even_cb(ud): ud.even_nums.append(ud.nums[ud.index]) return "succeeded" StateMachine.add("EVEN", CBState(even_cb), {"succeeded": "continue"}) # add odd state @smach.cb_interface( input_keys=["nums", "index", "odd_nums"], output_keys=["odd_nums"], outcomes=["succeeded"], ) def odd_cb(ud): ud.odd_nums.append(ud.nums[ud.index]) return "succeeded" StateMachine.add("ODD", CBState(odd_cb), {"succeeded": "continue"}) ## %EndTag(CONTAINER)% ## %Tag(ADDCONTAINER)% # close container_sm Iterator.set_contained_state("CONTAINER_STATE", container_sm, loop_outcomes=["continue"]) ## %EndTag(ADDCONTAINER)% ## %Tag(ADDITERATOR)% # close the tutorial_it StateMachine.add("TUTORIAL_IT", tutorial_it, { "succeeded": "succeeded", "aborted": "aborted" }) ## %EndTag(ADDITERATOR)% return sm
def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.init() # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.n_patrols = 2 # Turn the waypoints into SMACH states nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0)) # Initialize the top level state machine self.sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with self.sm: # Initialize the iterator self.sm_patrol_iterator = Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[], it=lambda: range(0, self.n_patrols), output_keys=[], it_label='index', exhausted_outcome='succeeded') with self.sm_patrol_iterator: # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'continue']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1', 'preempted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2', 'preempted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': 'NAV_STATE_3', 'aborted': 'NAV_STATE_3', 'preempted': 'NAV_STATE_3' }) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={ 'succeeded': 'NAV_STATE_4', 'aborted': 'NAV_STATE_4', 'preempted': 'NAV_STATE_4' }) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={ 'succeeded': 'continue', 'aborted': 'continue', 'preempted': 'continue' }) # Close the sm_patrol machine and add it to the iterator Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue']) # Close the top level state machine StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, { 'succeeded': 'succeeded', 'aborted': 'aborted' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def main(): rospy.init_node('fcsc_smach') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) sm.userdata.products = [] sm.userdata.sandwiches = [] sm.userdata.scrap_sandwiches = [] sm.userdata.normal_sandwiches = [] sm.userdata.damy_output = [] sm.userdata.adjust_count = 0; sm.userdata.faceup_count = 0; # Open the container with sm: def move_initial_pose_response_cb(userdata, response): if response.success == True: return 'succeeded' else: return 'aborted' smach.StateMachine.add('MOVE_INITIAL_POSE', ServiceState('move_initial_pose', Trigger, response_cb=move_initial_pose_response_cb), # transitions={'succeeded':'DETECT_AND_RECOVER_SANDWICH'}) transitions={'succeeded':'NAVIGATE_TO_SHELF_A'}) navigate_to_shelf_a_sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with navigate_to_shelf_a_sm: navigate_to_shelf_a_sm.add('GO_TO_NEAR_SHELF', ServiceState('goto_near_shelf', GoToNearShelf, request=GoToNearShelfRequest(0)), transitions={'succeeded':'DETECT_SHELF_A'}) navigate_to_shelf_a_sm.add('DETECT_SHELF_A', ServiceState('detect_shelf_action', DetectShelf, request=DetectShelfRequest(0)), transitions={'succeeded':'GO_TO_STOCKING_POSITION', 'aborted':'aborted'}) navigate_to_shelf_a_sm.add('GO_TO_STOCKING_POSITION', ServiceState('goto_stocking_base_position', Trigger), transitions={'succeeded':'succeeded'}) smach.StateMachine.add('NAVIGATE_TO_SHELF_A', navigate_to_shelf_a_sm, # transitions={'succeeded':'NAVIGATE_TO_SHELF_B'}) transitions={'succeeded':'DETECT_PRODUCT'}) smach.StateMachine.add('DETECT_PRODUCT', ServiceState('detect_product', DetectProduct, response_slots=['detected_object_array']), transitions={'succeeded':'STOCK_PRODUCTS'}, remapping={'detected_object_array':'products'}) stock_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['products', 'damy_output'], it=lambda: range(0, len(sm.userdata.products.objects)), output_keys=['damy_output'], it_label='index', exhausted_outcome='succeeded') with stock_it: stock_sm = smach.StateMachine(outcomes=['continue', 'succeeded', 'preempted', 'aborted'], input_keys=['index', 'products']) with stock_sm: @smach.cb_interface(input_keys=['products', 'index']) def pickup_request_cb(userdata, request): request.object = userdata.products.objects[userdata.index] return request @smach.cb_interface(outcomes=['succeeded', 'aborted']) def pickup_response_cb(userdata, response): if response.success == True: return 'succeeded' else: return 'aborted' smach.StateMachine.add('PICKUP_PRODUCT', ServiceState('pickup_product', Manipulate, request_cb=pickup_request_cb, response_cb=pickup_response_cb, input_keys=['products', 'index']), transitions={'succeeded':'PLACE_PRODUCT', 'aborted':'continue'}) @smach.cb_interface(input_keys=['products', 'index']) def place_request_cb(userdata, request): request.object = userdata.products.objects[userdata.index] return request @smach.cb_interface(outcomes=['succeeded', 'aborted']) def place_response_cb(userdata, response): if response.success: return 'succeeded' else: return 'aborted' smach.StateMachine.add('PLACE_PRODUCT', ServiceState('place_product', Manipulate, request_cb=place_request_cb, response_cb=place_response_cb, input_keys=['products', 'index']), transitions={'succeeded':'continue', 'aborted':'continue'}) @smach.cb_interface(input_keys=['products', 'index']) def return_request_cb(userdata, request): request.object = userdata.products.objects[userdata.index] return request @smach.cb_interface(outcomes=['succeeded', 'aborted']) def return_response_cb(userdata, response): if response.success: return 'succeeded' else: return 'aborted' smach.StateMachine.add('RETURN_PRODUCT', ServiceState('return_product', Manipulate, request_cb=return_request_cb, response_cb=return_response_cb, input_keys=['products', 'index']), transitions={'succeeded':'continue', 'aborted':'aborted'}) Iterator.set_contained_state('STOCK_PRODUCTS_STATE', stock_sm, loop_outcomes=['continue']) smach.StateMachine.add('STOCK_PRODUCTS', stock_it, transitions={'succeeded':'NAVIGATE_TO_SHELF_B', 'aborted':'aborted'}) navigate_to_shelf_b_sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with navigate_to_shelf_b_sm: navigate_to_shelf_b_sm.add('GO_TO_NEAR_SHELF_B', ServiceState('goto_near_shelf', GoToNearShelf, request=GoToNearShelfRequest(2)), transitions={'succeeded':'DETECT_SHELF_B'}) navigate_to_shelf_b_sm.add('DETECT_SHELF_B', ServiceState('detect_shelf_action', DetectShelf, request=DetectShelfRequest(1)), transitions={'succeeded':'GO_TO_FACEUP_POSITION', 'aborted':'aborted'}) navigate_to_shelf_a_sm.add('GO_TO_FACEUP_POSITION', ServiceState('goto_faceup_base_position', Trigger), transitions={'succeeded':'succeeded'}) smach.StateMachine.add('NAVIGATE_TO_SHELF_B', navigate_to_shelf_b_sm, transitions={'succeeded':'DETECT_AND_RECOVER_SANDWICH'}) smach.StateMachine.add('DETECT_AND_RECOVER_SANDWICH', ServiceState('detect_and_recover_sandwich', DetectAndRecoverSandwich, response_slots=['normal_sandwiches']), transitions={'succeeded':'FACEUP_SANDWICHES'}) @smach.cb_interface(input_keys=['normal_sandwiches']) def faceup_request_cb(userdata, request): request.sandwiches = userdata.normal_sandwiches return request @smach.cb_interface(outcomes=['succeeded', 'aborted']) def faceup_response_cb(userdata, response): if response.success: return 'succeeded' else: return 'aborted' smach.StateMachine.add('FACEUP_SANDWICHES', ServiceState('faceup_standing_sandwiches', FaceupSandwich, request_cb=faceup_request_cb, response_cb=faceup_response_cb, input_keys=['normal_sandwiches'] ), transitions={'succeeded':'GO_TO_HOME_POSITION'}) smach.StateMachine.add('GO_TO_HOME_POSITION', ServiceState('goto_home_position', Trigger), transitions={'succeeded':'succeeded'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() rospy.sleep(3) # Execute SMACH plan outcome = sm.execute() rospy.spin()
def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.init() # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.n_patrols = 2 # Turn the waypoints into SMACH states nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0)) # Initialize the top level state machine self.sm = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm: # Initialize the iterator self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = [], it = lambda: range(0, self.n_patrols), output_keys = [], it_label = 'index', exhausted_outcome = 'succeeded') with self.sm_patrol_iterator: # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'}) # Close the sm_patrol machine and add it to the iterator Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue']) # Close the top level state machine StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def main(): uci_topic = 'chessboard_uci_node' chesspiece_topic = 'chessboard_chesspieces_node' rospy.init_node('chessboard_sm_node') sm = smach.StateMachine( outcomes=['finished', 'succeeded', 'preempted', 'aborted']) sm.userdata.target = [] sis = smach_ros.IntrospectionServer('chessboard_sm_sis', sm, '/SM_ROOT') sis.start() with sm: sq_init = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') #Stockfish init with sq_init: rospy.loginfo('Stockfish init') Sequence.add( 'PIECE_RESET', SimpleActionState('chessboard_chesspieces_reset', ResetAction)) Sequence.add( 'UCI_INIT', SimpleActionState( uci_topic, ucicommandAction, goal=ucicommandGoal(cmd=ucicommandGoal.CMD_INIT))) Sequence.add( 'UCI_NEWGAME', SimpleActionState( uci_topic, ucicommandAction, goal=ucicommandGoal(cmd=ucicommandGoal.CMD_NEWGAME))) smach.StateMachine.add('STOCKFISH_SETUP', sq_init, transitions={'succeeded': 'UCI_ITER'}) #Test code tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[], it=lambda: range(0, 2), output_keys=[], it_label='index', exhausted_outcome='succeeded') with tutorial_it: container_sm = smach.StateMachine(outcomes=[ 'finished', 'succeeded', 'preempted', 'aborted', 'continue' ]) with container_sm: def calc_result_cb(userdata, status, result): if status == GoalStatus.SUCCEEDED: result_coords = list(result.result.data) print(result.result.data) new_target = [] # convert chess notation to indices # e.g. b8c6 -> 6,7,5,5 new_target.append( 7 - int(string.lowercase.index(result_coords[0]))) new_target.append(int(result_coords[1]) - 1) new_target.append( 7 - string.lowercase.index(result_coords[2])) new_target.append(int(result_coords[3]) - 1) #print(new_target) userdata.target = new_target return 'succeeded' smach.StateMachine.add( 'UCI_CALCMOVE', SimpleActionState( uci_topic, ucicommandAction, goal=ucicommandGoal(cmd=ucicommandGoal.CMD_CALC_MOVE), result_cb=calc_result_cb, output_keys=['target']), transitions={'succeeded': 'PIECE_MOVE'}, remapping={'target': 'user_data_target'}) smach.StateMachine.add( 'PIECE_MOVE', SimpleActionState(chesspiece_topic, MoveActionAction, goal_slots=['target']), transitions={'succeeded': 'UCI_MOVE'}, remapping={'target': 'user_data_target'}) smach.StateMachine.add( 'UCI_MOVE', SimpleActionState( uci_topic, ucicommandAction, goal=ucicommandGoal(cmd=ucicommandGoal.CMD_MOVE)), transitions={'succeeded': 'finished'}) Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['finished']) smach.StateMachine.add('UCI_ITER', tutorial_it, { 'succeeded': 'succeeded', 'aborted': 'aborted' }) # uci_node -> CMD_CALC_MOVE # uci_node -> CMD_MOVE #get new target position #movement #set position in uci outcome = sm.execute() rospy.spin() sis.stop()
def main(): rospy.init_node('tinker_mission_manipulation') trans = tf.TransformListener() rospy.loginfo("Waiting for tf ...") rospy.sleep(3) assert (len(trans.getFrameStrings()) > 0) state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: def kinect_callback(userdata, result): userdata.objects = [] objects = [] sum_x = 0 for obj in result.objects.objects: position = obj.pose.pose.pose.position if position.y > 0.5 or position.y < -0.5: continue _levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80] for l in _levels: if fabs(l - position.z) < 0.05: position.z = l + 0.05 if position.z > 1.1: position.z += 0.05 obj.header.stamp = rospy.Time(0) kinect_point = PointStamped(header=obj.header, point=position) odom_point = trans.transformPoint('odom', kinect_point) sum_x += odom_point.point.x rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x, odom_point.point.y, odom_point.point.z) objects.append(odom_point) avg_x = sum_x / len(objects) for from_point in objects: to_point = copy.deepcopy(from_point) to_point.point.x = avg_x to_point.point.z = find_div(from_point.point.z) userdata.objects.append({'from': from_point, 'to': to_point}) rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x, to_point.point.y, to_point.point.z) rospy.loginfo(colored('Total Object: %d','green'), len(objects)) return 'succeeded' StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded': 'Start_Button'}) StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'}) StateMachine.add('S_Kinect_Recognition', ServiceState(service_name='/kinect_find_objects', service_spec=FindObjects, input_keys=['objects'], output_keys=['objects'], response_cb=kinect_callback), transitions={'succeeded': 'Generate_Report'}) StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'), transitions={'succeeded': 'IT_Objects_Iterator'} ) objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['objects'], output_keys=[], it=lambda: state.userdata.objects, it_label='target', exhausted_outcome='succeeded') with objects_iterator: fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'], input_keys=['target'], connector_outcome='succeeded') with fetch_object_sequence: Sequence.add('Speak', SpeakState('New Object recognized')) Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'), transitions={'aborted':'continue'}) concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=['target']) with concurrence: Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from')) Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False)) Sequence.add('Move_Fetch_Concurrence', concurrence) Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE)) Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from')) Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to')) Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to')) Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'}) Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue']) # end of objects_iterator StateMachine.add('IT_Objects_Iterator', objects_iterator, transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'}) StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init), transitions={'succeeded': 'succeeded', 'aborted': 'aborted'}) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
def main(): rospy.init_node('tinker_mission_manipulation') trans = tf.TransformListener() rospy.loginfo("Waiting for tf ...") rospy.sleep(3) assert (len(trans.getFrameStrings()) > 0) state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: def kinect_callback(userdata, result): userdata.objects = [] objects = [] sum_x = 0 for obj in result.objects.objects: position = obj.pose.pose.pose.position if position.y > 0.5 or position.y < -0.5: continue _levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80] for l in _levels: if fabs(l - position.z) < 0.05: position.z = l + 0.05 if position.z > 1.1: position.z += 0.05 obj.header.stamp = rospy.Time(0) kinect_point = PointStamped(header=obj.header, point=position) odom_point = trans.transformPoint('odom', kinect_point) sum_x += odom_point.point.x rospy.loginfo( colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x, odom_point.point.y, odom_point.point.z) objects.append(odom_point) avg_x = sum_x / len(objects) for from_point in objects: to_point = copy.deepcopy(from_point) to_point.point.x = avg_x to_point.point.z = find_div(from_point.point.z) userdata.objects.append({'from': from_point, 'to': to_point}) rospy.loginfo( colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x, to_point.point.y, to_point.point.z) rospy.loginfo(colored('Total Object: %d', 'green'), len(objects)) return 'succeeded' StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded': 'Start_Button'}) StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={ 'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition' }) StateMachine.add('S_Kinect_Recognition', ServiceState(service_name='/kinect_find_objects', service_spec=FindObjects, input_keys=['objects'], output_keys=['objects'], response_cb=kinect_callback), transitions={'succeeded': 'Generate_Report'}) StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'), transitions={'succeeded': 'IT_Objects_Iterator'}) objects_iterator = Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['objects'], output_keys=[], it=lambda: state.userdata.objects, it_label='target', exhausted_outcome='succeeded') with objects_iterator: fetch_object_sequence = Sequence( outcomes=['succeeded', 'aborted', 'continue', 'preempted'], input_keys=['target'], connector_outcome='succeeded') with fetch_object_sequence: Sequence.add('Speak', SpeakState('New Object recognized')) Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'), transitions={'aborted': 'continue'}) concurrence = Concurrence( outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=['target']) with concurrence: Concurrence.add( 'Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from')) Concurrence.add( 'Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x, y: False)) Sequence.add('Move_Fetch_Concurrence', concurrence) Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE)) Sequence.add( 'Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from')) Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to')) Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to')) Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'}) Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue']) # end of objects_iterator StateMachine.add('IT_Objects_Iterator', objects_iterator, transitions={ 'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset' }) StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()