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!")

def get_move_around_smach():
    sm = StateMachine(outcomes=['preempted'])
    #sm.set_initial_state('CHECK_MOVEMENT')

    with sm:
        StateMachine.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach(),
                         transitions={'succeeded':'MOVE_RANDOMLY'})

        StateMachine.add('MOVE_RANDOMLY', move_base.get_random_goal_smach('/base_link'),
                         transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
                                      'aborted':'SLEEP_UNTIL_ENABLED'})

#        StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
#        StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1),    # mockup

    return sm

class RandomPatrol():
    def __init__(self):
        rospy.init_node('random_patrol', anonymous=False)
        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # Initialize a number of parameters and variables
        setup_task_environment(self)
        
        # Initialize the patrol state machine
        self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
        
        # Set the userdata.waypoints variable to the pre-defined waypoints
        self.sm_patrol.userdata.waypoints = self.waypoints

        # Add the states to the state machine with the appropriate transitions
        with self.sm_patrol:            
            StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
                             transitions={'succeeded':'NAV_WAYPOINT'},
                             remapping={'waypoint_out':'patrol_waypoint'})
            
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'PICK_WAYPOINT', 
                                          'aborted':'PICK_WAYPOINT', 
                                          'preempted':'PICK_WAYPOINT'},
                             remapping={'waypoint_in':'patrol_waypoint'})
            
        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = self.sm_patrol.execute()
        
        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
                
        intro_server.stop()

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        
        self.sm_patrol.request_preempt()
        
        self.cmd_vel_pub.publish(Twist())
        
        rospy.sleep(1)

    def __init__(self):
        rospy.init_node("follow")
        rospy.on_shutdown(self.shutdown)
        self.state = None

        self.sm_follow = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        #    总状态机follow
        with self.sm_follow:
            self.co_follow = Concurrence(
                outcomes=["succeeded", "preempted", "aborted"],
                default_outcome="succeeded",
                #                              outcome_cb = self.co_follow_outcome_cb ,
                #                                   child_termination_cb=self.co_follow_child_cb
            )
            with self.co_follow:
                Concurrence.add("mo_L", MonitorState("people_position_estimation", PositionMeasurement, self.pos_M_cb))
                Concurrence.add("nav", Nav2Waypoint())
            #                 self.sm_nav=StateMachine(outcomes=('succeeded','aborted','preempted'))
            #                 with self.sm_nav:
            # #                    StateMachine.add('wait',MonitorState('sr_data',Int16,self.wait_cb),transitions={'valid':'wait','invalid':'nav_speech','preempted':'nav_speech'})
            #
            #                     self.speech_nav=Concurrence(outcomes=['get_in','succeeded'],
            #                                                default_outcome='succeeded',
            #                                                 outcome_cb=self.speech_nav_outcome_cb,
            #                                                 child_termination_cb=self.speech_nav_child_termination_cb
            #                                                 )
            #                     with self.speech_nav:
            #                         Concurrence.add('speech',MonitorState('sr_data',Int16,self.nav_speech_cb))
            #                         Concurrence.add('nav', Nav2Waypoint())
            #                     StateMachine.add('nav_speech',self.speech_nav,transitions={'get_in':'Get_in',})

            #                     self.get_in_speech=Concurrence(outcomes=['get_out','succeeded'],
            #                                                 default_outcome='succeeded',
            #                                                 outcome_cb=self.speech_get_in_outcome_cb,
            #                                                 child_termination_cb=self.speech_get_in_child_termination_cb
            #
            #                                                    )
            #                     with self.get_in_speech:
            #                         Concurrence.add('speech',MonitorState('sr_data',Int16,self.get_in_speech_cb))
            #                         Concurrence.add('get_in',Get_in())
            #                     StateMachine.add('Get_in',self.get_in_speech,transitions={'get_out':'Get_out'})
            #                     StateMachine.add('Get_out',Get_out(), transitions={'succeeded':'nav_speech','preempted':'','aborted':'Get_out' })
            #                 Concurrence.add('Nav',self.sm_nav)
            StateMachine.add("Nav_top", self.co_follow)

        a = self.sm_follow.execute()

    def __init__(self):
        global  OBJECT_ID
        rospy.init_node("test")
        
        sm=StateMachine(outcomes=["succeeded","aborted","preempted","valid","invalid"])
                 
        self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction)
        arm_goal = xm_ArmHeightGoal()
        arm_goal.distance_x = 1
        arm_goal.distance_z = -0.2
        self.arm_state=SimpleActionState('xm_move_arm_height',xm_ArmHeightAction,goal=arm_goal,
                                         result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(60),
                                          server_wait_timeout=rospy.Duration(60))
        
        gripper_goal = xm_GripperCommandGoal()
        gripper_goal.command.position = 0.0 
#         gripper_goal.command.torque = 0.5
#         gripper_goal.header.frame
        gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal,
                                          result_cb=self.wrist_cb, exec_timeout=rospy.Duration(10),
                                          server_wait_timeout=rospy.Duration(10))
        
        
        wrist_goal=xm_WristPositionGoal()
        wrist_goal.angle=0
        wrist_state=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal,
                                          result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10),
                                          server_wait_timeout=rospy.Duration(10))
        self.monitor_times=1
        sm.userdata.ob=0
        with sm:
#             StateMachine.add("FIND_OBJECT1", Find_Object(),
#                              transitions={"succeeded": "", 'aborted': ''}, )
#             StateMachine.add("angle", ChangeAngular(angle=0.2), transitions={"succeeded":""})
#             StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':''})
#             StateMachine.add("HEIGHT",self.arm_state,transitions={"succeeded":''})
#             StateMachine.add("GRIP",gripper_state,transitions={"succeeded":''})
#                 StateMachine.add("AR_TARGET2", MonitorState("ar_pose_marker", AlvarMarkers,self.get_mark_pos_cb), transitions={"valid":"AR_TARGET2","invalid":""})
#                 StateMachine.add("WRIST", wrist_state, transitions={"succeeded":""})
            StateMachine.add("CHANGE_HEIGHT1", Lift_HEIGHT(height=0.0), transitions={"succeeded":""})
#             StateMachine.add("changemode", ChangeMode("arm"), transitions={"succeeded":""},)
        
#         for i in range(4):
        sm.execute()
#             OBJECT_ID+=1
        rospy.sleep(1)

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes = [succeeded,aborted,preempted], output_keys = {'name_of_person'})

        with self:

            def face_recognition(userdata, goal):
                face_recog_goal = FaceRecognitionGoal()
                face_recog_goal.order_id = 0
                face_recog_goal.order_argument = 'Referee'
                return face_recog_goal

            class FaceProcessData(smach.State):
                def __init__(self):
                    smach.State.__init__(self, outcomes=['succeeded', 'face_not_found', 'preempted', 'aborted'],
                         input_keys=['process_names', 'process_confidence'])

                def execute(self, userdata):
                    if len(userdata.process_names) > 0:
                        for confidence_value in userdata.process_confidence:
                            if confidence_value > MINIMUM_CONFIDENCE:
                                return 'succeeded'

                    return 'face_not_found'


            intro_text = "Please stay still while I recognise you."
                
            StateMachine.add('TTS_SAY_CALIB',
                                        SpeakActionState(intro_text),
                                        transitions={succeeded: 'FACE_RECOGNITION'})

            StateMachine.add('FACE_RECOGNITION', SimpleActionState('face_recognition', FaceRecognitionAction, goal_cb=face_recognition,
                                        result_slots=['order_id', 'names', 'confidence']),
                                        transitions={'succeeded': 'PROCESS_FACEDATA'},
                                        remapping={'names': 'LP_names', 'confidence': 'LP_confidence'})

            smach.StateMachine.add('PROCESS_FACEDATA', FaceProcessData(),
                                        transitions={'succeeded': 'TTS_SAY_FOUND',
                                                        'face_not_found': 'TTS_SAY_NOT_FOUND',
                                                        'aborted': 'TTS_SAY_CALIB'},

                                        remapping={'process_names': 'LP_names',
                                                     'process_confidence': 'LP_confidence',
                                                        })

            found_text = "I found you!"
            StateMachine.add('TTS_SAY_FOUND',
                                        SpeakActionState(found_text),
                                        transitions={'succeeded':'succeeded'},
                                        remapping = {'name_of_person':''})

            not_found_text = "Sorry, I wasn't looking for you..."
            StateMachine.add('TTS_SAY_NOT_FOUND',
                                        SpeakActionState(not_found_text),
                                        transitions={'succeeded': 'moveAround'})