Python smach_ros.IntrospectionServer类代码示例

    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