Example #1
1
    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()
Example #2
0
def main():
	#Create a rospy node for the state machine	
	rospy.init_node('IRA_statemachine')

	#Creating a state machine for banking scenario
	sm_bank = StateMachine(outcomes=['Stop']) 

	#Create Smach containers for each state
	with sm_bank:
		StateMachine.add('Face Recognition',
                     frn.Face_recognition(),
                     transitions={'Detected':'Speech_Gesture','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'})
		
		sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Speech1':'Completed','Gesture1':'Completed'}})

		with sm_con:
			smach.Concurrence.add('Speech1',ss1.speech())
			smach.Concurrence.add('Gesture1',g1.gesture())

		smach.StateMachine.add('Speech_Gesture', sm_con,
                                 transitions={'Waiting':'Stop',
                                              'Done':'Face Recognition'})
	
				

		
	
		StateMachine.add('Hotword_detection',
                     hwdc.Hotword_detection(),
                     transitions={'Detected':'Speech_Recognition','Not_Detected':'Face Recognition'})

		StateMachine.add('Speech_Recognition',
                     src.Speech_recognition(),
                     transitions={'Completed':'Speech_database','Not_Completed':'Face Recognition'},remapping={'speech_recognition_out':'Database_in'})

		StateMachine.add('Speech_database',
                     sdc.Speech_database(),
                     transitions={'Completed':'CON','Not_Completed':'Face Recognition'},remapping={'speech_database_out':'Text_in'})

		sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Text_speech':'Completed'}},input_keys=['Text_in'])
	
		with sm_con:
			smach.Concurrence.add('Text_speech',ts.Text_speech())
			#smach.Concurrence.add('ui1',ui1.Text_reply())
		

		StateMachine.add('CON',
                     sm_con,
                     transitions={'Waiting':'Stop','Done':'Hotword_detection'})
		

	#Code for initiating the smach_viewer
	Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW')
	Monitor.start()
	
	#Execute the state machine	
	outcome = sm_bank.execute()

	#Stop the smach_viewer if state machine stops
	Monitor.stop
Example #3
0
def search_for_tags(
        squares, marker,
        cube):  # type: (List[ParkingSquare], ARTag, ARCube) -> StateMachine
    sm = StateMachine(outcomes=['ok'])
    with sm:
        StateMachine.add('GO_TO_AR_SEARCH_1',
                         NavigateToNamedPoseState('ar_search_1'),
                         transitions={
                             'ok': 'SEARCH1',
                             'err': 'ABSORB'
                         })
        StateMachine.add('SEARCH1',
                         FindTags(squares, marker, cube),
                         transitions={
                             'ok': 'GO_TO_AR_SEARCH_2',
                             'found': 'ABSORB'
                         })
        StateMachine.add('GO_TO_AR_SEARCH_2',
                         NavigateToNamedPoseState('ar_search_2'),
                         transitions={
                             'ok': 'SEARCH2',
                             'err': 'ABSORB'
                         })
        StateMachine.add('SEARCH2',
                         FindTags(squares, marker, cube),
                         transitions={
                             'ok': 'GO_TO_AR_SEARCH_1',
                             'found': 'ABSORB'
                         })
        StateMachine.add('ABSORB', AbsorbResultState())
    return sm
def callback(message):
    global flag
    global sm
    if flag:
        kwargs = PoseStamped()
        kwargs.pose = message.workpoint
#        if message.statename == 'InitialState':
#            kwargs.pose = message.workpoint
#        elif message.statename == 'AfterGrasp':
#            kwargs.pose = message.workpoint
#        if message.keywords:
#            pass
#            kwargs = tuples_to_dict(message.keywords, message.arg)
#            kwargs.pose = message.grasp1
        with sm:
            StateMachine.add(
                message.id,
                globals()[
                    message.statename](kwargs),
                transitions=tuples_to_dict(
                    message.src,
                    message.dst))
        if message.is_end is True:
            flag = False
            while True:
                status = sm.execute()
                if status == 'success':
                    print('===SUCCESS!===')
                    print('===RESTART STATE MACHINE===')
                    os._exit(1)
                else:
                    print('###FAILED...###')
                    os._exit(1)
    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'}
                            )
Example #6
0
def main():
    rospy.init_node("fsm_dummy")

    sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        StateMachine.add('INITIAL_TURN',
                         SimpleActionState('/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()
Example #7
0
def main():
    rospy.init_node('smach_example_state_machine')

    # Create a SMACH state machine
    sm = StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_counter = 0

    sis = IntrospectionServer('example', sm, '/SM_ROOT')
    sis.start()

    # Open the container
    with sm:
        # Add states to the container
        StateMachine.add('FOO',
                         Foo(),
                         transitions={
                             'outcome1': 'BAR',
                             'outcome2': 'outcome4'
                         },
                         remapping={
                             'foo_counter_in': 'sm_counter',
                             'foo_counter_out': 'sm_counter'
                         })
        StateMachine.add('BAR',
                         Bar(),
                         transitions={'outcome1': 'FOO'},
                         remapping={'bar_counter_in': 'sm_counter'})

    # Execute SMACH plan
    outcome = sm.execute()
    rospy.spin()
Example #8
0
    def __init__(self):
        StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])

        with self:
            StateMachine.add('CROUCH_POSE', GoToPostureState('Crouch', 0.5), transitions={'succeeded': 'DISABLE_STIFF'})

            StateMachine.add('DISABLE_STIFF', DisableStiffnessState(), transitions={'succeeded': 'succeeded'})
Example #9
0
    def __init__(self, startPose='StandInit'):
        StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'])

        with self:
            StateMachine.add('ENABLE_STIFF', EnableStiffnessState(), transitions={'succeeded': 'START_POSITION'})

            StateMachine.add('START_POSITION', GoToPostureState(startPose, 0.5), transitions={'succeeded': 'succeeded'})
Example #10
0
    def __init__(self):
        rospy.init_node('go_and_follow')
        self.Test = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
        self.Test.userdata.PT_LIST = {}
        self.Test.userdata.mission = {}
        with self.Test:
            self.Test.userdata.targets = list()
            self.Test.userdata.actions = list()
            self.Test.userdata.people_condition = {}
            self.Test.userdata.command = 2
            # StateMachine.add('GET_TARGET',
            #                  CheckStop2(),
            #                  transitions={'stop': 'succeeded', 'aborted': 'GET_TARGET', 'remeber':'GET_TARGET'})

            StateMachine.add(
                'GET_TASK',
                GeneralAnswer(),
                transitions={
                    'succeeded': 'GET_TASK',
                    'aborted': 'aborted'
                },
                remapping={
                    'targets': 'targets',
                    'actions': 'actions',
                    #'task_num':'tasksNum'
                })

        self.Test.execute()
    def __init__(self):

        rospy.init_node('move_to_and_inspect_point_sm', anonymous=False)


        hsm = StateMachine(outcomes=['finished statemachine'])
        
        with hsm:

            StateMachine.add(   'GO_TO_POINT',
                        SimpleActionState(  'pid_global',
                                            MoveAction,
                                            makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)),
                                            transitions = { "succeeded": 'INSPECT_POINT',
                                                            "preempted": 'INSPECT_POINT', 
                                                            "aborted": 'INSPECT_POINT' })    
            StateMachine.add(   'INSPECT_POINT',
                         SimpleActionState( 'inspect_point',
                                            MoveAction,
                                            makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)),
                                            transitions = { 'succeeded': 'INSPECT_POINT',
                                                            "preempted": 'INSPECT_POINT', 
                                                            "aborted": 'INSPECT_POINT' })  
            
        

        


        intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT')
        intro_server.start()
        hsm.execute()
        #patrol.execute()
        print("State machine execute finished")
        intro_server.stop()
Example #12
0
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 __init__(self):
        StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
                              input_keys=['text'], output_keys=['text'])
        with self:
            StateMachine.add('START_LISTEN',
                             StartRecognitionState(),
                             transitions={'succeeded': 'ANSWER_DETECTION'}
                             )
            StateMachine.add('ANSWER_DETECTION',
                             GetRecognizedWordNoEmptyList(timeout=30),
                             transitions={'succeeded': 'STOP_LISTEN',
                                          'timeouted': 'STOP_LISTEN_ABORTED'},
                             remapping={'recognized_words': 'text'}
                             )

            StateMachine.add('STOP_LISTEN',
                             StopRecognitionState(),
                             transitions={'succeeded': 'GET_SINGLE_RESPONSE'}
                             )

            StateMachine.add('GET_SINGLE_RESPONSE',
                             CBState(self.list_cb, input_keys=['text'], output_keys=['text'], outcomes=['succeeded', 'aborted']),
                             transitions={'succeeded': 'succeeded', 'aborted': 'aborted'}
                             )

            StateMachine.add('STOP_LISTEN_ABORTED',
                             StopRecognitionState(),
                             transitions={'succeeded': 'aborted'}
                             )
Example #14
0
    def __init__(self):
        rospy.init_node('speech_recognition')
        rospy.on_shutdown(self.shutdown)
        self.find_people = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])
        self.test_bool = False
        with self.find_people:
            self.answer = Iterator(outcomes=['succeeded', 'aborted', 'error'],
                                   input_keys=['people_condition'],
                                   output_keys=[],
                                   it=lambda: range(0, 5),
                                   exhausted_outcome='succeeded')
            with self.answer:

                Iterator.set_contained_state('ANSWER_STATE',
                                             Anwser(),
                                             loop_outcomes=['succeeded'])
            StateMachine.add('ANSWER',
                             self.answer,
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })

        out = self.find_people.execute()
        if out == 'succeeded':
            self.test_bool = True
            rospy.logwarn('test finished, all thing is done well!')
        else:
            rospy.logerr('test failed ,something goes wrong')
Example #15
0
    def __init__(self):
        rospy.init_node('test_vision',anonymous=False)
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo('------------start test----------------')
        self.test_bool = False
        self.test_vision = StateMachine(outcomes=['succeeded','aborted','error'])
        with self.test_vision:
            StateMachine.add('RUN_NODE',
                                RunNode_obj(),
                                transitions={'succeeded':'WAIT','aborted':'aborted'})
            self.test_vision.userdata.rec = 20.0
            StateMachine.add('WAIT',
                                Wait(),
                                transitions={'succeeded':'succeeded','error':'error'})
            # StateMachine.add('CLOSENODE',
            #                     CloseKinect(),
            #                     transitions={'succeeded':'succeeded','aborted':'aborted'})


#            self.test_vision.userdata.pos_xm  =Pose()
#            StateMachine.add('GET_PEOPLE_POS',
#                                FindPeople().find_people_,
#                                transitions ={'invalid':'succeeded','valid':'error','preempted':'aborted'},
#                                remapping = {'pos_xm':'pos_xm'})
        
#        rospy.loginfo(self.test_vision.userdata.pos_xm)
        out = self.test_vision.execute()
#        if out =='error':
#            rospy.logerr('no position past!')
#            self.test_bool = False
#        else:
        self.test_bool = True
Example #16
0
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()
Example #17
0
    def __init__(self):

        rospy.init_node('test')
        rospy.on_shutdown(self.shutdown)

        self.top = StateMachine(outcomes=['succeeded', 'aborted', 'error'])

        with self.top:
            self.top.userdata.go_point = Point(1, 0, 0)
            self.top.userdata.degree = 3.14
            StateMachine.add('sm1',
                             LinearDisplacement(),
                             transitions={
                                 'succeeded': 'sm2',
                                 'preempted': 'sm2',
                                 'error': 'error'
                             },
                             remapping={'displacementVec': 'go_point'})

            StateMachine.add('sm2',
                             TurnDegree(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             },
                             remapping={'degree': 'degree'})

        self.top.execute()
Example #18
0
    def __init__(self):
        StateMachine.__init__(self,
                              outcomes=['succeeded', 'aborted', 'preempted'])

        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'map'
        pose = Pose()
        pose.position = Point(8.2, -3.0, 0.0)
        pose.orientation = Quaternion(0.0, 0.0, 1.0, 0.0)
        nav_goal.target_pose.pose = pose
        self.nav_docking_station = SimpleActionState('move_base',
                                                     MoveBaseAction,
                                                     goal=nav_goal)

        with self:
            StateMachine.add('NAVIGATE_TO_OUTLET',
                             self.nav_docking_station,
                             transitions={
                                 'succeeded': 'DOCKING',
                                 'aborted': 'NAVIGATE_TO_OUTLET'
                             })

            StateMachine.add('DOCKING',
                             ServiceState(
                                 'battery_simulator/set_battery_level',
                                 SetBatteryLevel, 100),
                             transitions={'succeeded': 'succeeded'})
Example #19
0
    def __init__(self):
        rospy.init_node('sm_Follow')

        self.trace = Concurrence(
            outcomes=['succeeded', 'aborted'],
            default_outcome='aborted',
            outcome_map={
                'succeeded': {
                    'STOP': 'stop'
                },
                'aborted': {
                    'FOLLOW': 'aborted'
                }
            },
            #                                 outcome_cb = self.trace_out_cb,
            child_termination_cb=self.trace_child_cb)
        with self.trace:
            self.meta_follow = StateMachine(['succeeded', 'aborted'])
            with self.meta_follow:
                StateMachine.add('FOLLOW',
                                 SimpleFollow(),
                                 transitions={
                                     'succeeded': 'FINDPEOPLE',
                                     'aborted': 'aborted'
                                 })
            Concurrence.add('RUNNODE', RunNode())
            Concurrence.add('FOLLOW', self.meta_follow)
            Concurrence.add('STOP', CheckStop())

        out = self.trace.execute()
        print out
Example #20
0
def main():
    rospy.init_node('smach_usecase_executive')

    sm_root = StateMachine(outcomes=['preempted', 'aborted', 'succeeded'])

    with sm_root:
        StateMachine.add('RESET',
                         ServiceState('/reset', Empty, request=EmptyRequest()),
                         transitions={'succeeded': 'SPAWN'})
        StateMachine.add(
            'SPAWN',
            ServiceState('/spawn', Empty, request=EmptyRequest()),
            #request=SpawnRequest(1.0, 1.0, 2.0, 'kkk')),
            transitions={'aborted': 'aborted'})


#            transitions={
#                'preemtped': 'preemtped',
#                'aborted': 'aborted',
#                'succeeded': 'succeeded'})

    sis = IntrospectionServer('exmaple', sm_root, '/USE_CASE')
    sis.start()

    outcome = sm_root.execute()

    rospy.spin()
Example #21
0
def cameraIdentificationSimple():
    def _termination_cb(outcome_map):
        return True

    sm = StateMachine(outcomes=['victim_approached', 'aborted', 'preempted'])

    with sm:

        StateMachine.add('GET_VICTIM',
                         SelectTargetState('victim'),
                         transitions={
                             'succeeded': 'GO_TO_VICTIM',
                             'aborted': 'aborted',
                             'preempted': 'preempted'
                         })

        StateMachine.add('GO_TO_VICTIM',
                         MoveBaseState(),
                         transitions={
                             'succeeded': 'victim_approached',
                             'aborted': 'GET_VICTIM',
                             'preempted': 'preempted'
                         })

    return sm
def main():
    rospy.init_node('smach_usecase_step_02')

    # Create a SMACH state machine
    sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])

    # Open the container
    with sm0:
        # Reset turtlesim
        StateMachine.add('RESET',
                ServiceState('reset', std_srvs.srv.Empty),
                {'succeeded':'SPAWN'})

        # Create a second turtle
        StateMachine.add('SPAWN',
                ServiceState('spawn', turtlesim.srv.Spawn,
                    request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')))

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

    # Execute SMACH tree
    outcome = sm0.execute()

    # Signal ROS shutdown (kill threads in background)
    rospy.spin()

    sis.stop()
Example #23
0
def construct_sm():
    sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        tutorial_it = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
                               it=lambda: range(0, 10),
                               it_label='index',
                               input_keys=[],
                               output_keys=[],
                               exhausted_outcome='succeeded')
        with tutorial_it:

            container_sm = utils.TargetSelectorContainer('explore')

            #close container_sm
            Iterator.set_contained_state('CONTAINER_STATE',
                                         container_sm,
                                         loop_outcomes=['target_sent'])

        #close the tutorial_it
        StateMachine.add('TUTORIAL_IT', tutorial_it, {
            'succeeded': 'succeeded',
            'aborted': 'aborted'
        })
    return sm
Example #24
0
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('exploration_fsm')

    sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        StateMachine.add('INITIAL_TURN',
                         InitialTurnState(),
                         transitions={
                             'succeeded': 'EXPLORATION',
                             'aborted': 'EXPLORATION',
                             'preempted': 'preempted'
                         })

        StateMachine.add('EXPLORATION',
                         simpleExplorationContainer(),
                         transitions={'preempted': 'preempted'})

    sis = smach_ros.IntrospectionServer('fsm_introspection', sm,
                                        '/EXPLORATION_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()
Example #26
0
    def __init__(self):
        rospy.init_node('turn')
        rospy.logwarn('ssssss')
        self.Turn = StateMachine(outcomes=['succeeded', 'aborted', 'error'])

        with self.Turn:
            self.Turn.userdata.degree = pi
            self.Turn.userdata.sentences = 'I want play a riddle game'
            StateMachine.add('SPEAK',
                             Speak(),
                             transitions={
                                 'succeeded': 'turn',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={'sentences': 'sentences'})
            StateMachine.add('turn',
                             TurnDegree(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={'degree': 'degree'})

        self.Turn.execute()
Example #27
0
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('/move_base',
                                           MoveBaseAction,
                                           goal_key='next_target'),
                         transitions={
                             'succeeded': 'target_sent',
                             'aborted': 'GET_TARGET',
                             'preempted': 'preempted'
                         })

    return sm_target_selector
    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()
Example #29
0
    def test_service_cb(self):
        """Test calling a service with a callback."""

        srv = rospy.Service('/empty', std_srvs.Empty, empty_server)

        sm = StateMachine(['succeeded', 'aborted', 'preempted', 'done'])
        with sm:

            def foo_response_cb(userdata, response):
                userdata.foo_var_out = 'foo!'
                return 'succeeded'

            StateMachine.add('FOO',
                             ServiceState('/empty',
                                          std_srvs.Empty,
                                          response_cb=foo_response_cb,
                                          output_keys=['foo_var_out']),
                             remapping={'foo_var_out': 'sm_var'},
                             transitions={'succeeded': 'done'})

        outcome = sm.execute()

        rospy.logwarn("OUTCOME: " + outcome)

        assert outcome == 'done'
def PatrolStateMachine(listOfWaypoints):
    tempWaypoint = ()
    waypoints = []
    #convert waypoints to ros topic format
    #waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)])
    for i in range(0, len(listOfWaypoints), 1):
        tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0])
        waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)])

    #start state machine
    patrol = StateMachine(['succeeded', 'aborted', 'preempted'])
    with patrol:
        for i, w in enumerate(waypoints):
            goal_pose = MoveBaseGoal()
            goal_pose.target_pose.header.frame_id = 'map'

            goal_pose.target_pose.pose.position.x = w[1][0]
            goal_pose.target_pose.pose.position.y = w[1][1]
            goal_pose.target_pose.pose.position.z = 0.0

            goal_pose.target_pose.pose.orientation.x = w[2][0]
            goal_pose.target_pose.pose.orientation.y = w[2][1]
            goal_pose.target_pose.pose.orientation.z = w[2][2]
            goal_pose.target_pose.pose.orientation.w = w[2][3]

            StateMachine.add(w[0],
                 SimpleActionState('move_base',
                       MoveBaseAction,
                       goal=goal_pose),
                 transitions={'succeeded': waypoints[(i + 1) % \
                          len(waypoints)][0]})
    patrol.execute()
Example #31
0
    def __init__(self):
        rospy.init_node('turn_test', anonymous=False)
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo('started test the turn')

        self.test_bool = False

        self.test_turn = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])

        with self.test_turn:
            self.test_turn.userdata.degree = -3.1415926 / 8
            StateMachine.add('TURN',
                             TurnDegree(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={'degree': 'degree'})

        out = self.test_turn.execute()
        rospy.loginfo(out)
        if out == 'succeeded':
            self.test_bool = True
Example #32
0
    def test_userdata(self):
        """Test serial manipulation of userdata."""

        pinger_thread = threading.Thread(target=pinger)
        pinger_thread.start()

        init_ud = UserData()
        init_ud.b = 'A'

        sm = StateMachine(['valid', 'invalid', 'preempted'])
        sm.set_initial_state(['MON'], userdata=init_ud)

        assert 'b' in sm.userdata
        assert sm.userdata.b == 'A'

        with sm:
            StateMachine.add(
                'MON',
                MonitorState('/ping',
                             Empty,
                             cond_cb,
                             input_keys=['b'],
                             output_keys=['a']))

        outcome = sm.execute()

        assert outcome == 'invalid'
        assert 'b' in sm.userdata
        assert sm.userdata.b == 'A'
        assert 'a' in sm.userdata
        assert sm.userdata.a == 'A'
Example #33
0
    def __init__(self):
        rospy.init_node('TestFollow')
        rospy.on_shutdown(self.shutdown)
        rospy.logerr('Follow start')

        self.smach_bool = False

        self.newFollow = StateMachine(outcomes = ['succeeded' , 'aborted','error'])

        with self.newFollow:

            StateMachine.add('RUNNODE' , RunNode() , transitions = {'succeeded':'FOLLOW' , 
                                                                    'aborted':'RUNNODE',
                                                                    })

            StateMachine.add('FOLLOW' ,FollowTest() , transitions = {'succeeded':'succeeded' ,
                                                                    'error':'error',
                                                                    'aborted':'aborted'} )

                        
        intro_server = IntrospectionServer('sm_gpsr',self.newFollow,'/SM_ROOT')
        intro_server.start()
        out = self.newFollow.execute()
        rospy.logerr(out)
        intro_server.stop()
        self.smach_bool = True 
    def __init__(self, robot, cupboard_id, cupboard_navigation_area,
                 required_items):
        StateMachine.__init__(self,
                              outcomes=["succeeded", "failed"],
                              output_keys=["item_picked"])

        cupboard = EdEntityDesignator(robot=robot, id=cupboard_id)

        with self:
            StateMachine.add(
                "NAVIGATE_TO_CUPBOARD",
                NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area},
                                   cupboard),
                transitions={
                    'arrived': 'PICK_ITEM_FROM_CUPBOARD',
                    'unreachable': 'failed',
                    'goal_not_defined': 'failed'
                })

            StateMachine.add("PICK_ITEM_FROM_CUPBOARD",
                             PickItemFromCupboardDrawer(
                                 robot, cupboard_id, required_items),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'failed': 'failed'
                             })
Example #35
0
 def build_state_machine(self):
     '''
     建立状态机
     '''
     if not self.sm_fetchtea is None:
         self.sm_fetchtea = None
     self.sm_fetchtea = StateMachine(
         outcomes=['succeeded', 'aborted', 'preempted'])
     with self.sm_fetchtea:
         StateMachine.add('START_TIP',
                          SpeaktoTipStart(),
                          transitions={
                              'succeeded': 'GET_LOCATION',
                              'aborted': 'END_SPEAK_TO_TIP_ABORT'
                          })
         StateMachine.add('GET_LOCATION',
                          GetLocation(),
                          transitions={
                              'succeeded': 'NAV_TO_WAYPOINT',
                              'aborted': 'END_SPEAK_TO_TIP_ABORT'
                          },
                          remapping={'waypoint_out': 'nav_waypoint'})
         StateMachine.add('NAV_TO_WAYPOINT',
                          Nav2Waypoint(),
                          transitions={
                              'succeeded': 'END_SPEAK_TO_TIP_SUCESS',
                              'aborted': 'END_SPEAK_TO_TIP_ABORT'
                          },
                          remapping={'waypoint_in': 'nav_waypoint'})
         StateMachine.add('END_SPEAK_TO_TIP_ABORT',
                          EndSpeaktoTipAbort(),
                          transitions={'succeeded': 'succeeded'})
         StateMachine.add('END_SPEAK_TO_TIP_SUCESS',
                          EndSpeaktoTipSucess(),
                          transitions={'succeeded': 'succeeded'})
    def __init__(self, robot):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        with self:
            StateMachine.add("FIND_CUP",
                             CustomFindCup(robot),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'FIND_CUP'})
Example #37
0
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
Example #38
0
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
Example #39
0
    def __init__(self, robot):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        arm = Designator(robot.leftArm)

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

            StateMachine.add("GRAB", SimpleGrab(robot, arm),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})
Example #40
0
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 __init__(self):
        StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['user_answer'], output_keys=['text'])
        with self:
            # DONE integrate movements, speech and retrieval from akinator service
            StateMachine.add('QUESTION',
                             AkinatorServiceState(),
                             transitions={'game_finished': 'aborted', 'continue_game': 'QUESTION_GESTURE'},
                             remapping={'resp_text': 'text', 'question_response': 'user_answer'}
                             )

            AskingPool = ['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']
            StateMachine.add('QUESTION_GESTURE',
                             SpeechGesture(wait_before_speak=1.5, behavior_pool=AskingPool),
                             transitions={'succeeded': 'succeeded'})
Example #42
0
    def __init__(self, robot, door_id):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        with self:
            StateMachine.add("NAVIGATE_TO_DOOR",
                             NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=door_id)),
                             transitions={'arrived': 'OPEN_DOOR',
                                          'unreachable': 'failed',
                                          'goal_not_defined': 'failed'})

            StateMachine.add("OPEN_DOOR",
                             OpenDoor(robot, door_id),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})
Example #43
0
    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})
Example #44
0
 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 construct_sm():
    sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
    sm.userdata.nums = range(25, 88, 3)
    sm.userdata.even_nums = []
    sm.userdata.odd_nums = []
    with sm:
## %Tag(ITERATOR)%
        tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
                               input_keys = ['nums', 'even_nums', 'odd_nums'],
                               it = lambda: range(0, len(sm.userdata.nums)),
                               output_keys = ['even_nums', 'odd_nums'],
                               it_label = 'index',
                               exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)% 
## %Tag(CONTAINER)%
        with tutorial_it:
            container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
                                        input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
                                        output_keys = ['even_nums', 'odd_nums'])
            with container_sm:
                #test wether even or odd
                StateMachine.add('EVEN_OR_ODD',
                                 ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, 
                                                input_keys=['nums', 'index']),
                                 {'true':'ODD',
                                  'false':'EVEN' })
                #add even state
                @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
                                    output_keys=['odd_nums'], 
                                    outcomes=['succeeded'])
                def even_cb(ud):
                    ud.even_nums.append(ud.nums[ud.index])
                    return 'succeeded'
                StateMachine.add('EVEN', CBState(even_cb), 
                                 {'succeeded':'continue'})
                #add odd state
                @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], 
                                    output_keys=['odd_nums'], 
                                    outcomes=['succeeded'])
                def odd_cb(ud):
                    ud.odd_nums.append(ud.nums[ud.index])
                    return 'succeeded'
                StateMachine.add('ODD', CBState(odd_cb), 
                                 {'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
            #close container_sm
            Iterator.set_contained_state('CONTAINER_STATE', 
                                         container_sm, 
                                         loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
        #close the tutorial_it
        StateMachine.add('TUTORIAL_IT',tutorial_it,
                     {'succeeded':'succeeded',
                      'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
    return sm
    def __init__(self, robot, dishwasher_id, dishwasher_navigate_area):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id)

        with self:
            StateMachine.add("NAVIGATE_TO_DISHWASHER",
                             NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher),
                             transitions={'arrived': 'OPEN_DISHWASHER',
                                          'unreachable': 'failed',
                                          'goal_not_defined': 'failed'})

            StateMachine.add("OPEN_DISHWASHER",
                             OpenDishwasher(robot, dishwasher_id),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})
Example #47
0
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
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
Example #49
0
    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()
Example #50
0
    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)
Example #51
0
    def __init__(self, robot):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        @cb_interface(outcomes=['succeeded'], output_keys=['position'])
        def send_point(ud):
            ud.position = PointStamped(header=Header(frame_id='/amigo/base_link'), point=Point(0.5, 0, 0.7))
            return 'succeeded'

        arm = Designator(robot.leftArm)

        with self:
            StateMachine.add("SEND_POINT", CBState(send_point),
                             transitions={'succeeded': 'FIND_CUP'})

            StateMachine.add("FIND_CUP", SimpleGrab(robot, arm),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})
    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'})
Example #53
0
 def __init__(self):
     State.__init__(self, outcomes=['succeeded','aborted','preempted'])
     setup_task_environment(self);
     # Initialize the navigation state machine
     self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
     self.sm_nav.userdata.waypoints = self.waypoints
     with self.sm_nav:            
         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':''},
                          remapping={'waypoint_in':'patrol_waypoint'})
     
     pass
Example #54
0
def create_find_clusters_sm():
    sm = StateMachine(outcomes=['succeeded','aborted'],
                      output_keys=['segmentation_result','cluster_information','action_index'])
                      
    with sm:
        StateMachine.add('RESET_ROBOT',
                         ResetRobot(),
                         remapping={'action_index':'action_index'},
                         transitions={'succeeded':'SEGMENT_SCENE'})
                          
        StateMachine.add('SEGMENT_SCENE',
                         SegmentScene(),
                         remapping={'segmentation_result':'segmentation_result',
                                    'cluster_information':'cluster_information'},
                         transitions={'found_clusters':'succeeded',
                                      'no_clusters':'aborted'})
                                      
    return sm
Example #55
0
    def __init__(self, keyPointManager):
        State.__init__(self, outcomes=['succeeded','aborted','preempted'])
        self.keyPointManager = keyPointManager

        self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        

        with self.sm_nav:            
            StateMachine.add('PICK_KEYPOINT', PickWaypointKeyPoint(self.keyPointManager),
                             transitions={'succeeded':'NAV_KEYPOINT'},
                             remapping={'waypoint_out':'patrol_waypoint'})
            
            StateMachine.add('NAV_KEYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'PICK_KEYPOINT', 
                                          'aborted':'PICK_KEYPOINT', 
                                          'preempted':''},
                             remapping={'waypoint_in':'patrol_waypoint'})
        
        pass