Python smach.StateMachine类代码示例

    def __init__(self):
        Concurrence(default_outcome='aborted',
                    input_keys=['text'],
                    output_keys=['text'],
                    outcomes=['succeeded', 'preempted', 'aborted'],
                    child_termination_cb=self.getfinish_Cb,
                    outcome_cb=self.outcome_Cb)

        jointLoop = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        with jointLoop:
            StateMachine.add('NEXT_MOVE',
                             RandomSelectionFromPoolState(self._movementList),
                             output_keys={'joint_angles'},
                             transitions={'succeeded': 'MOVEMENT'}
                             )
            StateMachine.add('MOVEMENT',
                             JointAngleState(['HeadPitch', 'HeadYaw']),
                             transitions={'succeeded': 'NEXT_MOVE'}
                             )

        with self:
            Concurrence.add('MOVING',
                            jointLoop,
                            transitions={'succeeded': 'succeeded'}
                            )

            Concurrence.add('GET_USER_ANSWER',
                            GetUserAnswer(),
                            transitions={'succeeded': 'succeeded',
                                         'aborted': 'aborted'},
                            remapping={'text': 'text'}
                            )

def main():
    rospy.init_node("fsm_dummy")

    sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
    with sm:
        StateMachine.add('INITIAL_TURN', SimpleActionState('/navigation/initial_turn', InitialTurnAction), 
        transitions={'succeeded':'EXPLORATION','aborted':'aborted','preempted':'preempted'})
        
        StateMachine.add('EXPLORATION', exploration.ExplorationContainer(), 
        transitions={'victim_thermal':'IDENTIFICATION_SIMPLE',
        'victim_camera':'IDENTIFICATION_TRACKING','aborted':'aborted','preempted':'preempted','time_out':'AGGRESSIVE_EXPLORATION'})
        
        StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(), 
        transitions={'parked':'succeeded','aborted':'aborted','preempted':'preempted'})
        
        StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(), 
        transitions={'identification_finished':'succeeded','aborted':'aborted','preempted':'preempted'})
        
        StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction),
        transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'})

    sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM')
    sis.start()
    
    smach_ros.set_preempt_handler(sm)

    # Execute SMACH tree in a separate thread so that we can ctrl-c the script
    smach_thread = threading.Thread(target = sm.execute)
    smach_thread.start()
        
    rospy.spin()
    sis.stop()

def ExplorationContainer():
	cc = Concurrence(outcomes=['victim_thermal','victim_camera','aborted','preempted','time_out'], 
	default_outcome='aborted', outcome_map={'victim_thermal':{'VICTIM_MONITOR':'victim_thermal'}, 
	'victim_camera':{'VICTIM_MONITOR':'victim_camera'},'preempted':{'EXPLORE':'preempted','VICTIM_MONITOR':'preempted'}, 
	'aborted':{'EXPLORE':'aborted'}, 'time_out':{'EXPLORE':'time_out'}},
	child_termination_cb=_termination_cb)
	
	with cc:
		#~ Concurrence.add('TARGET_CONTROLLER', utils.TargetSelectorContainer('explore'))
		Concurrence.add('EXPLORE', utils.make_iterator(utils.TargetSelectorContainer('explore'), max_iter=100))
		
		sm_victim_monitor = StateMachine(outcomes=['victim_thermal','victim_camera','preempted'])
		sm_victim_monitor.userdata.victim_type = 0
		with sm_victim_monitor:
			
			StateMachine.add('VICTIM_MONITORING', MonitorVictimState(
			input_keys=['victim_type'], output_keys=['victim_type']), 
			transitions={'invalid':'VICTIM_DECIDE', 'valid':'VICTIM_MONITORING', 'preempted':'preempted'}, 
			remapping={'victim_type':'victim_type'})
			
			StateMachine.add('VICTIM_DECIDE', DecideVictimState(), 
			transitions={'thermal':'victim_thermal','camera':'victim_camera'})
			
		Concurrence.add('VICTIM_MONITOR', sm_victim_monitor)
		
	return cc

def main():
    rospy.init_node('smach_example_state_machine')

    sm = StateMachine(['exit'])
    with sm:
        for key, (x, y, next_point) in WAYPOINTS.items():
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = 'map'
            goal.target_pose.pose.position.x = x
            goal.target_pose.pose.position.y = y
            goal.target_pose.pose.orientation.w = 1.

            StateMachine.add(key, SimpleActionState('move_base',
                                                MoveBaseAction,
                                                goal=goal),
                              transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'})

    # Create and start the introspection server
    sis = IntrospectionServer('strat', sm, '/SM_ROOT')
    sis.start()

    # Execute the state machine
    outcome = sm.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()

    def __init__(self, robot):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        with self:
            StateMachine.add("FIND_CUP",
                             CustomFindCup(robot),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'FIND_CUP'})

def IdentificationTrackingContainer():
	
	sm_identification_tracking = StateMachine(['identification_finished','aborted','preempted'])
	
	with sm_identification_tracking:
		
		StateMachine.add('DO_NOTHING', SimpleActionState('/navigation/initial_turn', InitialTurnAction), 
		transitions={'succeeded':'identification_finished','aborted':'aborted','preempted':'preempted'})
	
	return sm_identification_tracking

def create_root_sm():
    """ Create root State Machine with meta state (sleeping, awake) """
    root_sm = StateMachine(
        outcomes=[]
    )

    with root_sm:
        StateMachine.add('SLEEPING', Sleeping(),
                         transitions={'sensing': 'AWAKE'})
        StateMachine.add('AWAKE', create_awake_sm(),
                         transitions={'bored': 'SLEEPING'})
    return root_sm

    def __init__(self, robot, arm):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=['position'])

        @cb_interface(outcomes=['succeeded'])
        def prepare_grasp(ud):
            # Open gripper (non-blocking)
            arm.resolve().send_gripper_goal('open', timeout=0)

            # Torso up (non-blocking)
            robot.torso.high()

            # Arm to position in a safe way
            arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0)

            # Open gripper
            arm.resolve().send_gripper_goal('open', timeout=0.0)
            return 'succeeded'

        with self:
            StateMachine.add("PREPARE_GRASP", CBState(prepare_grasp),
                             transitions={'succeeded': 'SIMPLE_NAVIGATE_TO_GRASP'})

            StateMachine.add("SIMPLE_NAVIGATE_TO_GRASP", SimpleNavigateToGrasp(robot, arm),
                             transitions={'unreachable': 'failed',
                                          'arrived': 'SIMPLE_PICKUP',
                                          'goal_not_defined': 'failed'})

            StateMachine.add("SIMPLE_PICKUP", SimplePickup(robot, arm),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})

def main():
    rospy.init_node('tinker_mission_follow')
    rospy.loginfo(colored('starting follow and guide task ...', 'green'))

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
        StateMachine.add('1_Start', MonitorKinectBodyState(), 
                transitions={'valid':'1_Start', 'invalid':'Sequence'})
        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
        with sequence:
            follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])

            with follow_concurrence:
                Concurrence.add('FollowMe', FollowMeState())
                Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))

            Sequence.add('Follow_concurrence', follow_concurrence)
	   
        StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})

    # Run state machine introspection server for smach viewer
    intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
    intro_server.start()

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()

    def __init__(self, wait_time=3.0):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
                                    input_keys=[],
                                    output_keys=[])

        with self:
            @smach.cb_interface(outcomes=[succeeded])
            def wait(userdata):
                rospy.sleep(wait_time)
                return succeeded

            StateMachine.add('WAIT_STATE', CBState(wait, outcomes=[succeeded]), transitions={succeeded: 'TAKE_SNAPSHOT'})

            StateMachine.add('TAKE_SNAPSHOT', ServiceState('/xtion_snapshotter/snapshot', Empty),
                             transitions={succeeded: succeeded})

 def add_states(self):
     sm_nested = StateMachine(outcomes=self.outcomes, input_keys=self.input_keys, output_keys=self.output_keys)
     machine_count = 0
     state_count = 0
     StateMachine.add(self.title,sm_nested,transitions=self.transitions, remapping=self.remapping)
     with sm_nested:
         for thing in self.things_to_add:
             if thing == 's':
                 addition = self.state_machine_additions[state_count]
                 StateMachine.add(addition.title, addition.state, addition.transitions, remapping=addition.remapping)
                 state_count += 1
             else:
                 machine = self.state_machines[machine_count]
                 machine.add_states()
                 machine_count += 1

 def __init__(self, distance_threshold):
     StateMachine.__init__(self, outcomes=['detected','failed','preempted'], input_keys=['sm_distance_threshold'], output_keys=['sm_pose'])
     
     #self.userdata.sm_distance_threshold = 
     #self.userdata.required_still_checks = 
     
     
     with self:
         smach.StateMachine.add('DETECT_CLOSE_PERSON', DetectClosePersonState(distance_threshold),
                                  transitions = {'check_again':'DETECT_CLOSE_PERSON', 'done':'WAIT_FOR_STILL_PERSON', 'failed':'failed', 'preempted':'preempted'},
                                  remapping = {'pose':'sm_pose'})
         
         smach.StateMachine.add('WAIT_FOR_STILL_PERSON', WaitForStillPersonState(2),
                                transitions = {'check_again':'DETECT_CLOSE_PERSON','done':'detected','failed':'DETECT_CLOSE_PERSON','preempted':'preempted'},
                                remapping = {'pose':'sm_pose'})

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 TargetSelectorContainer(target_type):
	
	sm_target_selector = StateMachine(['target_sent','aborted','preempted'])
	
	with sm_target_selector:
		
		target_selection_goal = targetSelectionGoal(target_type)
		StateMachine.add('GET_TARGET', SimpleActionState('/select_target', SelectTargetAction, 
		goal=target_selection_goal, result_key='target_pose'), 
		transitions={'succeeded':'MOVE_BASE','aborted':'aborted','preempted':'preempted'}, 
		remapping={'target_pose':'next_target'})
		
		StateMachine.add('MOVE_BASE', SimpleActionState('/navigation/move_base', MoveBaseAction, goal_key='next_target'), 
		transitions={'succeeded':'target_sent','aborted':'aborted','preempted':'preempted'})
		
	return sm_target_selector

class TestGripperSmach:
    def __init__(self):
        rospy.init_node("test_gripper_smach", anonymous=False)
        gripper_goal = xm_GripperCommandGoal()
        gripper_goal.command.position = 0.10
        gripper_goal.command.torque = 0.5
        gripper_state = SimpleActionState(
            "xm_move_gripper",
            xm_GripperCommandAction,
            goal=gripper_goal,
            result_cb=self.gripper_state_cb,
            exec_timeout=rospy.Duration(10),
            server_wait_timeout=rospy.Duration(10),
        )
        self.gripper_smach = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with self.gripper_smach:
            StateMachine.add(
                "GRIPPER_OPEN", gripper_state, transitions={"succeeded": "", "aborted": "", "preempted": ""}
            )
        gripper_outcome = self.gripper_smach.execute()
        rospy.loginfo("State Machine Outcome: " + str(gripper_outcome))

    def gripper_state_cb(self, userdata, status, result):
        if result:
            rospy.loginfo("Complete!")