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
        setup_task_environment(self)
        
        # Track success rate of getting to the goal locations
        self.n_succeeded = 0
        self.n_aborted = 0
        self.n_preempted = 0

        # A list to hold then navigation waypoints
        nav_states = list()
        
        # Turn the waypoints into SMACH states
        for waypoint in self.waypoints:           
            nav_goal = MoveBaseGoal()
            nav_goal.target_pose.header.frame_id = 'base_footprint'
            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)
        
        # Initialize the patrol state machine
        self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])

        # 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':'','aborted':'','preempted':''})
            
        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine for the specified number of patrols
        while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
            sm_outcome = self.sm_patrol.execute()
            self.patrol_count += 1
            rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
        
        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
                
        intro_server.stop()

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

    sauvc = smach.StateMachine(outcomes=['succeeded','preempted'])

    with sauvc:
                                                                 
        smach.StateMachine.add('COUNTDOWN_START',Countdown(1.0),transitions={'succeeded':'SMACH_TRUE'})
        
        smach.StateMachine.add('SMACH_TRUE',SmachSwitch(True),transitions={'succeeded':'VISION_TRACK_TRUE'}) 
        smach.StateMachine.add('VISION_TRACK_TRUE',TrackerSwitch(False),transitions={'succeeded':'NAV_TRUE'})                                        
        smach.StateMachine.add('NAV_TRUE',NavigationSwitch(True),transitions={'succeeded':'TOPSIDE_FALSE'})                                       
        smach.StateMachine.add('TOPSIDE_FALSE',TopsideSwitch(False),transitions={'succeeded':'COUNTDOWN_DIVE'})  
                              
        smach.StateMachine.add('COUNTDOWN_DIVE',Countdown(1),transitions={'succeeded':'VISION_TRACK_TRUE1'})
        smach.StateMachine.add('VISION_TRACK_TRUE1',TrackerSwitch(True),transitions={'succeeded':'NAV_TRUE1'})                                        
        smach.StateMachine.add('NAV_TRUE1',NavigationSwitch(False),transitions={'succeeded':'COUNTDOWN_MISSION2'})                                       

        smach.StateMachine.add('COUNTDOWN_MISSION2',Countdown(180),transitions={'succeeded':'TOPSIDE_TRUE'})

        smach.StateMachine.add('TOPSIDE_TRUE',TopsideSwitch(True),transitions={'succeeded':'COUNTDOWN_END'})
        smach.StateMachine.add('COUNTDOWN_END',Countdown(0.1),transitions={'succeeded':'VISION_TRACK_FALSE'})
        smach.StateMachine.add('VISION_TRACK_FALSE',TrackerSwitch(False),transitions={'succeeded':'NAV_FALSE'})                                        
        smach.StateMachine.add('NAV_FALSE',NavigationSwitch(False),transitions={'succeeded':'SMACH_FALSE'})                  
        smach.StateMachine.add('SMACH_FALSE',SmachSwitch(False),transitions={'succeeded':'succeeded'})   
                                        
    sis = IntrospectionServer('my_introspserver', sauvc,'/SM_ROOT')
    sis.start()
    outcome = sauvc.execute()
    rospy.on_shutdown(shutdown)
        
    rospy.spin()
    sis.stop()

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

    sm0 = StateMachine(outcomes=['succeeded','not successful'])
    
    sm0.userdata.action=''
    sm0.userdata.pose=Pose()
    
    with sm0:
        smach.StateMachine.add('UI_detection', UI_detection(),
                                      transitions={'outcome1':'Answer'})
        smach.StateMachine.add('Answer', Answer(), 
                               transitions={'success':'succeeded','retry':'not successful'})
                     

    # Attach a SMACH introspection server
    sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
    sis.start()

    # Set preempt handler
    #smach.set_preempt_handler(sm0)

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

    # Signal handler
    rospy.spin()

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 main():
    rospy.init_node('smach_example_state_machine')

    init_robot_pose()
    move_base_override.init()

    reset_robot_actuators()

    odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)

    sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
                  connector_outcome=Transitions.SUCCESS)
    with sq:
        Sequence.add('waiting', WaitStartState())

        for i in range(3):
            Sequence.add('fishing {}'.format(i), create_fish_sequence())
#        Sequence.add('inner_door', create_door_state_machine(0.3))
#        Sequence.add('outer_door', create_door_state_machine(0.6))

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

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

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

    def __init__(self):
        rospy.init_node('monitor_fake_battery', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        
        # Set the low battery threshold (between 0 and 100)
        self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
 
        # Initialize the state machine
        sm_battery_monitor = StateMachine(outcomes=[])

        with sm_battery_monitor:
            # Add a MonitorState to subscribe to the battery level topic
            StateMachine.add('MONITOR_BATTERY',
                 MonitorState('battery_level', Float32, self.battery_cb), 
                 transitions={'invalid':'RECHARGE_BATTERY',
                              'valid':'MONITOR_BATTERY',
                              'preempted':'MONITOR_BATTERY'},)

            # Add a ServiceState to simulate a recharge using the set_battery_level service
            StateMachine.add('RECHARGE_BATTERY',
                 ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100), 
                 transitions={'succeeded':'MONITOR_BATTERY',
                              'aborted':'MONITOR_BATTERY',
                              'preempted':'MONITOR_BATTERY'})
            
        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = sm_battery_monitor.execute()
                        
        intro_server.stop()

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):
        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
        setup_task_environment(self)
        
        # Track success rate of getting to the goal locations
        self.n_succeeded = 0
        self.n_aborted = 0
        self.n_preempted = 0
        self.patrol_count = 0
        self.n_patrols = 1
        
        # A random number generator for use in the transition callback
        self.rand = Random()
        
        # A list to hold then nav_states
        nav_states = list()
        
        # Turn the waypoints into SMACH states
        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)
        
        # Initialize the patrol state machine
        self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
        
        # Register a callback function to fire on state transitions within the sm_patrol state machine
        self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[])
        
        # 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'})
            StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
            StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
            StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
            StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'NAV_STATE_0','aborted':'NAV_STATE_0'})
            StateMachine.add('CHECK_COUNT', CheckCount(self.n_patrols), transitions={'succeeded':'NAV_STATE_0','aborted':'','preempted':''})

        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        self.sm_outcome = self.sm_patrol.execute()
        
        rospy.loginfo('State Machine Outcome: ' + str(self.sm_outcome))
                
        intro_server.stop()
        
        os._exit(0)

def main():
	rospy.init_node("iterator_tutorial")
	sm_iterator = construct_sm()

	# Run state machine introspection server for smach viewer
	intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
	intro_server.start()
	outcome = sm_iterator.execute()
	rospy.spin()
	intro_server.stop()

def main():
    rospy.init_node('register_head_ellipse')
    smer = SMEllipsoidRegistration()
    sm = smer.get_sm()
    rospy.sleep(1)

    sis = IntrospectionServer('register_head', sm, '/UNFREEZE_PC')
    sis.start()

    outcome = sm.execute()
    
    sis.stop()

def main():
    rospy.init_node('pr2_touch_simple')
    smts = SMTouchSimple()
    sm = smts.get_sm_basic()
    rospy.sleep(1)

    sis = IntrospectionServer('touch_simple', sm, '/INPUT_START_CLICK')
    sis.start()

    outcome = sm.execute()
    
    sis.stop()

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

    smna = SMNavApproach()
    sm = smna.get_sm()
    rospy.sleep(1)

    sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
    sis.start()

    outcome = sm.execute()
    
    sis.stop()

def main():
    rospy.init_node('tinker_mission_navigation')
    rospy.loginfo(colored('starting navigation task ...', 'green'))

    # load waypoints from xml
    pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
    for pose in pose_list.pose:
        WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})

        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence:
#            Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
#            Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
#            Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), 
#                    transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'}) 
#            Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
#            Sequence.add('ObstacleDelay', DelayState(10),
#                    transitions={'succeeded': 'GoToWaypoin2'}) 
#            Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
            Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
            Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
            Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
            Sequence.add('Train_human', FollowTrainState())

            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)
            Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
            Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
            Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
	   
        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 main():
    rospy.init_node("sm_registration_setup")
    smrs = SMRegistrationSetup()
    sm = smrs.get_sm()
    if False:
        sm.set_initial_state(['SETUP_TASK_CONTROLLER'])
    rospy.sleep(1)

    sis = IntrospectionServer('registration_setup', sm, '/NAV_APPROACH')
    sis.start()

    outcome = sm.execute()
    
    sis.stop()

def execute_smach_container(smach_container, enable_introspection=False,
                            name='/SM_ROOT', userdata=UserData()):
    if not rospy.core.is_initialized():
        rospy.init_node('smach')

    if enable_introspection:
        # Create and start the introspection server
        sis = IntrospectionServer('smach_executor', smach_container, name)
        sis.start()

    outcome = smach_container.execute(userdata)
    _logger.info("smach outcome: %s", outcome)

    if enable_introspection:
        sis.stop()

    return outcome

def main():
    rospy.init_node('tinker_mission_person_recognition')
    rospy.loginfo(colored('starting person recognition task ...', 'green'))

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'})
        StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'})
        StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'), 
                transitions={'succeeded': '1_Start'})
        StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'})
        sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                            connector_outcome='succeeded')
        with sequence:
            Sequence.add('2_1_train_speak', SpeakState('please look at the camera'))
            Sequence.add('2_2_train', TrainPersonState())
            Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks'))
            Sequence.add('3_wait', DelayState(10))
            Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23)) 
            Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0)) 
            Sequence.add('5_1_find_operator', FindPersonState()) 
            Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'}) 
            Sequence.add('5_3_say', SpeakState('I have found you'))
            Sequence.add('5_4_wait', DelayState(5))

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

        sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                            connector_outcome='succeeded')
        with sequence_reset:
            Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init))
            Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt'))
            Sequence.add('6_3_finish', SpeakState('task finished'))

        StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'})

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

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

def main():
    rospy.init_node('tinker_RIPS')
    rospy.loginfo(colored('starting RIPS task ...', 'green'))

    # load waypoints from xml
    pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
    for pose in pose_list.pose:
        WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)

    # Main StateMachine
    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})

        sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence_1:
            Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
            Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue'))

        StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'})
        StateMachine.add('Continue', MonitorStartButtonState(), 
                transitions={'valid': 'Continue', 'invalid': 'Sequence_2'})
        sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
                connector_outcome='succeeded')
        with sequence_2:
            Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'}) 
            Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
            Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
            Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
        StateMachine.add('Sequence_2', sequence_2, {'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):
        rospy.init_node('final_project_kl', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        self.keyPointManager = KeyPointManager()
        
         # Create the nav_patrol state machine using a Concurrence container
        self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'],
                                        default_outcome='succeeded',
                                        outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}},
                                        child_termination_cb=self.concurrence_child_termination_cb,
                                        outcome_cb=self.concurrence_outcome_cb)
        # Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine             
        with self.nav_patrol:
           Concurrence.add('SM_NAV', Patrol().getSM())
           Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
        
        
        # Create the top level state machine
        self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])   
        self.sm_top.userdata.sm_ar_tag = None

        with self.sm_top:
            StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'}) 
            StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) 
            
            
        intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = self.sm_top.execute()
        
        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
                
        intro_server.stop()

    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()

#.........这里部分代码省略.........
#####################################

	# State machine for JO
        self.sm_jo = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_jo:
	    StateMachine.add('AWAKE', self.sm_jo_awake, transitions={'succeeded':'succeeded', 'stop':'aborted', 'go_sleep':'SLEEP', 'go_out':'OUT'})
	    StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={'succeeded':'succeeded', 'wake_up':'AWAKE'})
	    StateMachine.add('OUT', self.sm_jo_out, transitions={'succeeded':'succeeded', 'back_home':'AWAKE'})


#####################################
# TOP LVL CAROLE SM        TODO
#####################################


        # State machine for CAROLE
        self.sm_carole = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_carole:
	    StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
            StateMachine.add('PAUSE', Pause(),
                             transitions={'succeeded':'WAIT3',
                                          'aborted':'aborted'})



#####################################
# TOP LVL EAT SM        TODO
#####################################


        # State machine for EAT
        self.sm_eat = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_eat:
	    StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
            StateMachine.add('PAUSE', Pause(),
                             transitions={'succeeded':'WAIT2',
                                          'aborted':'aborted'})


#####################################
# TOP LVL SHOWER SM 
#####################################



        # State machine for SHOWER
        self.sm_shower = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_shower:
	    StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={'valid':'PREPARING_SHOWER', 'preempted':'preempted', 'invalid':'PREPARING_SHOWER'})
            StateMachine.add('PREPARING_SHOWER', PreparingShower(),
                             transitions={'succeeded':'GO_SHOWER',
                                          'aborted':'WAIT1'})
            StateMachine.add('GO_SHOWER', GoShower(),
                             transitions={'succeeded':'STOP_SHOWER',
                                          'aborted':'aborted'})
            StateMachine.add('STOP_SHOWER', StopShower(),
                             transitions={'succeeded':'WAIT1',
                                          'aborted':'aborted'})



#####################################
# TOP LVL SM 
#####################################


        # Create the top level state machine
        self.sm_top = Concurrence(outcomes=['succeeded', 'stop'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.concurrence_child_termination_cb,
                                        outcome_cb=self.concurrence_outcome_cb)

        # Add nav_patrol, sm_recharge and a Stop() machine to sm_top
        with self.sm_top:
	    Concurrence.add('SM_JO', self.sm_jo)
	    Concurrence.add('SM_CAROLE', self.sm_carole)
	    Concurrence.add('SM_EAT', self.sm_eat)
	    Concurrence.add('SM_SHOWER', self.sm_shower)








        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = self.sm_top.execute()
        
        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
                
        intro_server.stop()