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'}
                            )
Exemple #2
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 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
Exemple #4
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()
    def __init__(self, robot):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"])

        with self:
            StateMachine.add("FIND_CUP",
                             CustomFindCup(robot),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'FIND_CUP'})
def IdentificationTrackingContainer():
	
	sm_identification_tracking = StateMachine(['identification_finished','aborted','preempted'])
	
	with sm_identification_tracking:
		
		StateMachine.add('DO_NOTHING', SimpleActionState('/navigation/initial_turn', InitialTurnAction), 
		transitions={'succeeded':'identification_finished','aborted':'aborted','preempted':'preempted'})
	
	return sm_identification_tracking
Exemple #7
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
    def __init__(self, robot, arm):
        StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=['position'])

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

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

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

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

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

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

            StateMachine.add("SIMPLE_PICKUP", SimplePickup(robot, arm),
                             transitions={'succeeded': 'succeeded',
                                          'failed': 'failed'})
def main():
    rospy.init_node('tinker_mission_follow')
    rospy.loginfo(colored('starting follow and guide task ...', 'green'))

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

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

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

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

    outcome = state.execute()
    rospy.spin()
    intro_server.stop()
    def __init__(self, wait_time=3.0):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
                                    input_keys=[],
                                    output_keys=[])

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

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

            StateMachine.add('TAKE_SNAPSHOT', ServiceState('/xtion_snapshotter/snapshot', Empty),
                             transitions={succeeded: succeeded})
Exemple #11
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 __init__(self, distance_threshold):
     StateMachine.__init__(self, outcomes=['detected','failed','preempted'], input_keys=['sm_distance_threshold'], output_keys=['sm_pose'])
     
     #self.userdata.sm_distance_threshold = 
     #self.userdata.required_still_checks = 
     
     
     with self:
         smach.StateMachine.add('DETECT_CLOSE_PERSON', DetectClosePersonState(distance_threshold),
                                  transitions = {'check_again':'DETECT_CLOSE_PERSON', 'done':'WAIT_FOR_STILL_PERSON', 'failed':'failed', 'preempted':'preempted'},
                                  remapping = {'pose':'sm_pose'})
         
         smach.StateMachine.add('WAIT_FOR_STILL_PERSON', WaitForStillPersonState(2),
                                transitions = {'check_again':'DETECT_CLOSE_PERSON','done':'detected','failed':'DETECT_CLOSE_PERSON','preempted':'preempted'},
                                remapping = {'pose':'sm_pose'})
def construct_sm():
    sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
    sm.userdata.nums = range(25, 88, 3)
    sm.userdata.even_nums = []
    sm.userdata.odd_nums = []
    with sm:
## %Tag(ITERATOR)%
        tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
                               input_keys = ['nums', 'even_nums', 'odd_nums'],
                               it = lambda: range(0, len(sm.userdata.nums)),
                               output_keys = ['even_nums', 'odd_nums'],
                               it_label = 'index',
                               exhausted_outcome = 'succeeded')
## %EndTag(ITERATOR)% 
## %Tag(CONTAINER)%
        with tutorial_it:
            container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
                                        input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
                                        output_keys = ['even_nums', 'odd_nums'])
            with container_sm:
                #test wether even or odd
                StateMachine.add('EVEN_OR_ODD',
                                 ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, 
                                                input_keys=['nums', 'index']),
                                 {'true':'ODD',
                                  'false':'EVEN' })
                #add even state
                @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
                                    output_keys=['odd_nums'], 
                                    outcomes=['succeeded'])
                def even_cb(ud):
                    ud.even_nums.append(ud.nums[ud.index])
                    return 'succeeded'
                StateMachine.add('EVEN', CBState(even_cb), 
                                 {'succeeded':'continue'})
                #add odd state
                @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], 
                                    output_keys=['odd_nums'], 
                                    outcomes=['succeeded'])
                def odd_cb(ud):
                    ud.odd_nums.append(ud.nums[ud.index])
                    return 'succeeded'
                StateMachine.add('ODD', CBState(odd_cb), 
                                 {'succeeded':'continue'})
## %EndTag(CONTAINER)%
## %Tag(ADDCONTAINER)%
            #close container_sm
            Iterator.set_contained_state('CONTAINER_STATE', 
                                         container_sm, 
                                         loop_outcomes=['continue'])
## %EndTag(ADDCONTAINER)%
## %Tag(ADDITERATOR)%
        #close the tutorial_it
        StateMachine.add('TUTORIAL_IT',tutorial_it,
                     {'succeeded':'succeeded',
                      'aborted':'aborted'})
## %EndTag(ADDITERATOR)%
    return sm
def get_move_around_smach():
    sm = StateMachine(outcomes=['preempted'])
    #sm.set_initial_state('CHECK_MOVEMENT')

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

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

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

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

    def gripper_state_cb(self, userdata, status, result):
        if result:
            rospy.loginfo("Complete!")
Exemple #16
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
class RandomPatrol():
    def __init__(self):
        rospy.init_node('random_patrol', anonymous=False)
        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # Initialize a number of parameters and variables
        setup_task_environment(self)
        
        # Initialize the patrol state machine
        self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
        
        # Set the userdata.waypoints variable to the pre-defined waypoints
        self.sm_patrol.userdata.waypoints = self.waypoints

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

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        
        self.sm_patrol.request_preempt()
        
        self.cmd_vel_pub.publish(Twist())
        
        rospy.sleep(1)
Exemple #18
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)
Exemple #19
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()
    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'})
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
 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
    def __init__(self):
        smach.StateMachine.__init__(self,[succeeded,aborted,preempted])

        with self:

            def move_around(userdata, goal):
                moveGoal = MoveBaseGoal()
                moveGoal.target_pose.header.stamp = rospy.Time.now()
                moveGoal.target_pose.header.frame_id = 'base_link'
                moveGoal.target_pose.pose.position.x = random.randint(-2, 2)
                moveGoal.target_pose.pose.position.y = random.randint(-2, 2)
                moveGoal.target_pose.pose.position.z = 0
                rotationAngle = random.random()
                moveGoal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, rotationAngle))

                print moveGoal

                return moveGoal

            # 2_1 is unnecessary
            smach.StateMachine.add(
                'FINDING',
                FindPersonStateMachine(),
                transitions ={'aborted':'MOVE_AROUND_DUMMY','succeeded':'THERE_IS_A_PERSON'})

            smach.StateMachine.add(
                'MOVE_AROUND_DUMMY',
                MoveAroundCounter(),
                transitions ={'aborted':'aborted','succeeded':'MOVE_AROUND'})

            StateMachine.add('MOVE_AROUND', 
                SimpleActionState('move_base', MoveBaseAction, goal_cb=move_around),
                transitions={'aborted': 'aborted','succeeded':'FINDING'})

            smach.StateMachine.add(
                'THERE_IS_A_PERSON',
                SpeakActionState("I found a person."),
                transitions={'succeeded': 'MOVE', 'aborted': 'MOVE'})

            smach.StateMachine.add(
                'MOVE',
                MoveToPerson(),
                transitions = {'succeeded':'succeeded'})
    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
    def __init__(self):
        super(PatrolController, self).__init__('PatrolServer')

        self.__keyListener = rospy.Subscriber('keys', String, self.__keysCallback)
        #TODO make sure shuffling works and thet the first node stays the same
        # Or try a flag to block the reissue of patrol

        self.__waypoints = self.__loadWaypointsFromCsv()
        self.__homeGoal = self.__getGoal(self.__waypoints[0])

        self.__StateMachine = StateMachine(['succeded', 'aborted', 'preempted'])
    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()
Exemple #27
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'})
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()
Exemple #29
0
def poly(sides):
    poly = StateMachine(outcomes=['success'])
    for i in range(0,sides-1):
        StateMachine.add('SIDE{0}'.format(i+1),Drive(1),transitions={'success':'TURN{0}'.format(i+1)})
        StateMachine.add('TURN{0}'.format(i+1),Turn(360.0/sides),transitions={'success':'SIDE{0}'.format(i+2)})
    StateMachine.add('SIDE3',Drive(1),transitions={'success':'success'})
    return poly
Exemple #30
0
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('petit_smach_ai', anonymous=False)

        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)

        # Create a list to hold the target quaternions (orientations)
        quaternions = list()

        # First define the corner orientations as Euler angles
        euler_angles = (pi / 2, pi, 3 * pi / 2, 0)

        # Then convert the angles to quaternions
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)

        # Create a list to hold the waypoint poses
        self.waypoints = list()

        self.square_size = 1.0

        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
        self.waypoints.append(
            Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
        self.waypoints.append(
            Pose(Point(self.square_size, self.square_size, 0.0),
                 quaternions[1]))
        self.waypoints.append(
            Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))

        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)

        self.stopping = False
        self.recharging = False

        # State machine for Action1
        self.sm_action1 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])
        self.sm_action1.userdata.speed = 0.1
        self.sm_action1.userdata.distance = 1.0

        with self.sm_action1:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'FORWARD',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('FORWARD',
                             MoveForward(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action2
        self.sm_action2 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])
        self.sm_action2.userdata.speed = -0.1
        self.sm_action2.userdata.distance = 0.5

        with self.sm_action2:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'SIDE',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('SIDE',
                             MoveSide(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action3
        self.sm_action3 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action3:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action4
        self.sm_action4 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action4:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action5
        self.sm_action5 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action5:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action6
        self.sm_action6 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action6:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Actions
        self.sm_actions = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])
        self.sm_actions.userdata.waypoints = self.waypoints

        with self.sm_actions:
            StateMachine.add('PICK_WAYPOINT',
                             ServiceState('/PETIT/get_objective',
                                          GetObjective,
                                          response_cb=self.objective_cb,
                                          output_keys=['waypoint_out'],
                                          outcomes=[
                                              'action1', 'action2', 'action3',
                                              'action4', 'action5', 'action6',
                                              'aborted', 'succeeded',
                                              'preempted'
                                          ]),
                             transitions={
                                 'action1': 'SM_ACTION1',
                                 'action2': 'SM_ACTION2',
                                 'action3': 'SM_ACTION3',
                                 'action4': 'SM_ACTION4',
                                 'action5': 'SM_ACTION5',
                                 'action6': 'SM_ACTION6',
                                 'aborted': 'SM_ACTION1'
                             },
                             remapping={'waypoint_out': 'patrol_waypoint'})
            #StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
            #                 transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
            #                 remapping={'waypoint_out':'patrol_waypoint'})
            StateMachine.add('SM_ACTION1',
                             self.sm_action1,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION2',
                             self.sm_action2,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION3',
                             self.sm_action3,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION4',
                             self.sm_action4,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION5',
                             self.sm_action5,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION6',
                             self.sm_action6,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('REMOVE_OBJECTIVE',
                             RemoveObjective(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             },
                             remapping={'waypoint_in': 'remove_waypoint'})

# State machine with concurrence
        self.sm_concurrent = Concurrence(
            outcomes=['succeeded', 'stop'],
            default_outcome='succeeded',
            child_termination_cb=self.concurrence_child_termination_cb,
            outcome_cb=self.concurrence_outcome_cb)

        # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
        with self.sm_concurrent:
            Concurrence.add('SM_ACTIONS', self.sm_actions)
            Concurrence.add(
                'MONITOR_TIME',
                MonitorState("/GENERAL/remain", Int32, self.time_cb))
            Concurrence.add('MONITOR_BATTERY',
                            MonitorState("/PETIT/adc", Int32, self.battery_cb))

        # Create the top level state machine
        self.sm_top = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        # Add nav_patrol, sm_recharge and a Stop() machine to sm_top
        with self.sm_top:
            StateMachine.add('CONCURRENT',
                             self.sm_concurrent,
                             transitions={
                                 'succeeded': 'CONCURRENT',
                                 'stop': 'STOP'
                             })
            #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})

        # 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()
Exemple #32
0
if __name__ == '__main__':

    rospy.init_node('HUMABOT_SHOPPING_LIST_TEST')

    # Define needed nodes
    # Nodes names to check
    TOPIC_LIST_NAMES = ['/nao_robot/camera/top/camera/image_raw']
    SERVICES_LIST_NAMES = [
        '/nao_shopping_list/checkObjects', '/nao_shopping_list/trainObjects',
        '/cmd_pose_srv'
    ]
    ACTION_LIST_NAMES = ['/speech', '/joint_angles_action']
    PARAMS_LIST_NAMES = []

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

    with sm:

        StateMachine.add('CHECK_NODES',
                         CheckNodesState(TOPIC_LIST_NAMES, SERVICES_LIST_NAMES,
                                         ACTION_LIST_NAMES, PARAMS_LIST_NAMES),
                         transitions={
                             'succeeded': 'START_THE_TEST',
                             'aborted': 'aborted'
                         })

        StateMachine.add('START_THE_TEST',
                         StartTest(testName='Shopping list',
                                   dist_m_to_square=0.4,
Exemple #33
0
    def __init__(self, cart_area, cart_sub_area, action_req, action_server):
        StateMachine.__init__(self, outcomes=['done', 'failed'])

        # get cart pose state params
        map_frame_name = rospy.get_param('~map_frame_name', 'map')

        # get cart setpoint state params
        robot_length_m = float(rospy.get_param('~robot_length_m', '0.73'))
        cart_length_m = float(rospy.get_param('~cart_length_m', '0.81'))
        distance_to_cart_m = float(
            rospy.get_param('~distance_to_cart_m', '1.0'))

        # align and approach cart state params
        offset_to_approach_pose_m = float(
            rospy.get_param('~offset_to_approach_pose_m', '0.55'))
        backward_vel_docking_ms = float(
            rospy.get_param('~backward_vel_docking_ms', '0.1'))
        max_rot_vel_docking_rads = float(
            rospy.get_param('~max_rot_vel_docking_rads', '0.1'))
        approach_x_thresh_m = float(
            rospy.get_param('~approach_x_thresh_m', '0.05'))
        approach_y_thresh_m = float(
            rospy.get_param('~approach_y_thresh_m', '0.05'))
        approach_yaw_thresh_rad = float(
            rospy.get_param('~approach_yaw_thresh_rad', '0.1'))

        # couple to cart state params
        max_coupling_attempts = int(
            rospy.get_param('max_coupling_attempts', '3'))

        # post dock params
        post_dock_forward_distance_m = float(
            rospy.get_param('~post_dock_forward_distance_m', '0.2'))

        self.userdata.cart_pose = None
        self.userdata.pre_dock_setpoint = None
        self.userdata.post_dock_setpoint = None
        self.userdata.cart_area = cart_area
        self.userdata.cart_sub_area = cart_sub_area
        self.userdata.action_req = action_req
        self.userdata.action_server = action_server
        self.userdata.area_shape = None
        self.userdata.sub_area_shape = None

        with self:
            StateMachine.add('GET_CART_POSE',
                             GetCartPose(map_frame_name=map_frame_name),
                             transitions={
                                 'cart_found': 'GET_SETPOINT_IN_PRE_DOCK_AREA',
                                 'cart_not_found': 'LOOK_FOR_CART',
                                 'timeout': 'failed'
                             })

            StateMachine.add('GET_SETPOINT_IN_PRE_DOCK_AREA',
                             GetSetpointInPreDockArea(
                                 robot_length_m=robot_length_m,
                                 cart_length_m=cart_length_m,
                                 distance_to_cart_m=distance_to_cart_m),
                             transitions={
                                 'setpoint_found': 'GO_TO_PRE_DOCK_SETPOINT',
                                 'setpoint_unreachable': 'failed',
                                 'timeout': 'failed'
                             })

            StateMachine.add('GO_TO_PRE_DOCK_SETPOINT',
                             GoToPreDockSetpoint(),
                             transitions={
                                 'reached_setpoint': 'ALIGN_AND_APPROACH_CART',
                                 'setpoint_unreachable': 'failed',
                                 'timeout': 'failed'
                             })

            StateMachine.add(
                'ALIGN_AND_APPROACH_CART',
                AlignAndApproachCart(
                    offset_to_approach_pose_m=offset_to_approach_pose_m,
                    backward_vel_docking_ms=backward_vel_docking_ms,
                    max_rot_vel_docking_rads=max_rot_vel_docking_rads,
                    approach_x_thresh_m=approach_x_thresh_m,
                    approach_y_thresh_m=approach_y_thresh_m,
                    approach_yaw_thresh_rad=approach_yaw_thresh_rad),
                transitions={
                    'approach_succeeded': 'COUPLE_TO_CART',
                    'cart_not_found': 'GET_CART_POSE',
                    'cart_pose_publisher_not_available': 'failed',
                    'timeout': 'GO_TO_PRE_DOCK_SETPOINT'
                })

            StateMachine.add(
                'COUPLE_TO_CART',
                CoupleToCart(max_coupling_attempts=max_coupling_attempts),
                transitions={
                    'coupling_succeeded': 'GET_SETPOINT_IN_POST_DOCK_AREA',
                    'coupling_failed': 'GO_TO_PRE_DOCK_SETPOINT',
                    'cannot_switch_to_load_mode': 'failed'
                })

            StateMachine.add(
                'GET_SETPOINT_IN_POST_DOCK_AREA',
                GetSetpointInPostDockArea(
                    distance_to_move=post_dock_forward_distance_m),
                transitions={
                    'setpoint_found': 'GO_TO_POST_DOCK_SETPOINT',
                    # we consider the action a success even if it doesn't move forward post docking
                    'setpoint_unreachable': 'done',
                    'timeout': 'done'
                })

            StateMachine.add('GO_TO_POST_DOCK_SETPOINT',
                             GoToPostDockSetpoint(),
                             transitions={
                                 'reached_setpoint': 'done',
                                 'setpoint_unreachable': 'done',
                                 'timeout': 'done'
                             })
            #############################################################################################################
            ## Non-nominal states; i.e. states to execute when the nominal execution fails

            StateMachine.add('LOOK_FOR_CART',
                             LookForCart(map_frame_name=map_frame_name,
                                         robot_length_m=robot_length_m),
                             transitions={
                                 'cart_found': 'GET_SETPOINT_IN_PRE_DOCK_AREA',
                                 'cart_not_found': 'failed',
                                 'timeout': 'failed'
                             })
    def create_statemachine(self):
        try:
            response = self.relative_work_point_srv()
        except rospy.ServiceException as e:
            rospy.logerr('servive call failed:{}'.format(e))

        relative_grasp_pose = PoseStamped()
        relative_grasp_pose2 = PoseStamped()
        relative_grasp_pose3 = PoseStamped()
        relative_grasp_pose4 = PoseStamped()
        relative_grasp_pose5 = PoseStamped()
        relative_grasp_pose6 = PoseStamped()
        relative_assemble_pose = PoseStamped()
        relative_assemble_pose2 = PoseStamped()
        relative_assemble_pose3 = PoseStamped()
        relative_assemble_pose4 = PoseStamped()
        relative_assemble_pose5 = PoseStamped()
        relative_assemble_pose6 = PoseStamped()

        relative_grasp_pose.pose = response.grasp[0]
        relative_grasp_pose2.pose = response.grasp[1]
        relative_grasp_pose3.pose = response.grasp[2]
        relative_grasp_pose4.pose = response.grasp[3]
        relative_grasp_pose5.pose = response.grasp[4]
        relative_grasp_pose6.pose = response.grasp[5]
        relative_assemble_pose.pose = response.assemble[0]
        relative_assemble_pose2.pose = response.assemble[1]
        relative_assemble_pose3.pose = response.assemble[2]
        relative_assemble_pose4.pose = response.assemble[3]
        relative_assemble_pose5.pose = response.assemble[4]
        relative_assemble_pose6.pose = response.assemble[5]

        sm_top = StateMachine(outcomes=['success', 'failed'])
        with sm_top:
            StateMachine.add(
                "InitialState",
                InitialState(relative_grasp_pose),
                transitions={
                    'success': 'SUB1',
                    'failed': 'failed'})

            sm_sub1 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub1:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose2),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB1", sm_sub1, transitions={
                    'success': 'SUB2', 'failed': 'failed'})

            sm_sub2 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub2:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose2),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose2),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose3),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB2", sm_sub2, transitions={
                    'success': 'SUB3', 'failed': 'failed'})

            sm_sub3 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub3:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose3),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose3),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose4),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB3", sm_sub3, transitions={
                    'success': 'SUB4', 'failed': 'failed'})

            sm_sub4 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub4:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose4),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose4),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose5),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB4", sm_sub4, transitions={
                    'success': 'SUB5', 'failed': 'failed'})

            sm_sub5 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub5:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose5),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose5),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose6),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB5", sm_sub5, transitions={
                    'success': 'SUB6', 'failed': 'failed'})

            sm_sub6 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub6:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose6),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose6),
                    transitions={
                        'success': 'AfterAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssemble",
                    AfterAssemble(),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB6",
                sm_sub6,
                transitions={
                    'success': 'FinalState',
                    'failed': 'failed'})

            StateMachine.add(
                "FinalState",
                FinalState(),
                transitions={
                    'success': 'success',
                    'failed': 'failed'})

        sis = IntrospectionServer('p_and_p_example', sm_top, '/SM_ROOT')
        sis.start()

        sm_top.execute()
        rospy.spin()
Exemple #35
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()
Exemple #36
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
Exemple #37
0
def polygonial():

    # Construct static goals
    polygon_big = turtle_actionlib.msg.ShapeGoal(edges=11, radius=4.0)
    polygon_small = turtle_actionlib.msg.ShapeGoal(edges=6, radius=0.5)
    ##########################
    # 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')),
            {'succeeded': 'TELEPORT1'})

        # Teleport turtle 1
        StateMachine.add(
            'TELEPORT1',
            ServiceState('turtle1/teleport_absolute',
                         turtlesim.srv.TeleportAbsolute,
                         request=turtlesim.srv.TeleportAbsoluteRequest(
                             5.0, 1.0, 0.0)), {'succeeded': 'DRAW_SHAPES'})

        # Draw some polygons
        shapes_cc = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                                default_outcome='aborted',
                                outcome_map={
                                    'succeeded': {
                                        'BIG': 'succeeded',
                                        'SMALL': 'succeeded'
                                    }
                                })
        StateMachine.add('DRAW_SHAPES', shapes_cc)
        with shapes_cc:
            # Draw a large polygon with the first turtle
            Concurrence.add(
                'BIG',
                SimpleActionState('turtle_shape1',
                                  turtle_actionlib.msg.ShapeAction,
                                  goal=polygon_big))

            # Draw a small polygon with the second turtle
            small_shape_sm = StateMachine(
                outcomes=['succeeded', 'aborted', 'preempted'])
            Concurrence.add('SMALL', small_shape_sm)
            with small_shape_sm:
                # Teleport turtle 2
                StateMachine.add(
                    'TELEPORT2',
                    ServiceState('turtle2/teleport_absolute',
                                 turtlesim.srv.TeleportAbsolute,
                                 request=turtlesim.srv.TeleportAbsoluteRequest(
                                     9.0, 5.0, 0.0)),
                    {'succeeded': 'DRAW_WITH_MONITOR'})

                # Construct a concurrence for the shape action and the monitor
                draw_monitor_cc = Concurrence(
                    ['succeeded', 'aborted', 'preempted', 'interrupted'],
                    'aborted',
                    child_termination_cb=lambda so: True,
                    outcome_map={
                        'succeeded': {
                            'DRAW': 'succeeded'
                        },
                        'preempted': {
                            'DRAW': 'preempted',
                            'MONITOR': 'preempted'
                        },
                        'interrupted': {
                            'MONITOR': 'invalid'
                        }
                    })

                StateMachine.add('DRAW_WITH_MONITOR', draw_monitor_cc,
                                 {'interrupted': 'WAIT_FOR_CLEAR'})

                with draw_monitor_cc:
                    Concurrence.add(
                        'DRAW',
                        SimpleActionState('turtle_shape2',
                                          turtle_actionlib.msg.ShapeAction,
                                          goal=polygon_small))

                    def turtle_far_away(ud, msg):
                        """Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
                        if sqrt(pow(msg.x - 9.0, 2) +
                                pow(msg.y - 5.0, 2)) > 2.0:
                            return True
                        return False

                    Concurrence.add(
                        'MONITOR',
                        MonitorState('/turtle1/pose',
                                     turtlesim.msg.Pose,
                                     cond_cb=turtle_far_away))

                StateMachine.add(
                    'WAIT_FOR_CLEAR',
                    MonitorState(
                        '/turtle1/pose',
                        turtlesim.msg.Pose,
                        cond_cb=lambda ud, msg: not turtle_far_away(ud, msg)),
                    {
                        'valid': 'WAIT_FOR_CLEAR',
                        'invalid': 'TELEPORT2'
                    })

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

    # Set preempt handler
    smach_ros.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()
    ##################################
    # Construct action server wrapper
    asw = ActionServerWrapper(
        'my_action_server_name',
        MachineAction,
        wrapped_container=sm0,
        succeeded_outcomes=['did_something', 'did_something_else'],
        aborted_outcomes=['aborted'],
        preempted_outcomes=['preempted'])

    # Run the server in a background thread
    asw.run_server()
Exemple #38
0
    def __init__(self):
        rospy.init_node('follow')
        rospy.logerr('gogogo')

        self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
                                 default_outcome='stop',
                                 outcome_map={
                                     'remeber': {
                                         'STOP': 'remeber'
                                     },
                                     'stop': {
                                         'STOP': 'stop'
                                     },
                                     'aborted': {
                                         'FOLLOW': 'aborted'
                                     }
                                 },
                                 child_termination_cb=self.trace_child_cb,
                                 input_keys=['PT_LIST', 'mission'],
                                 output_keys=['PT_LIST', 'mission'])
        with self.trace:
            self.meta_follow = StateMachine(
                ['succeeded', 'aborted', 'preempted'])
            with self.meta_follow:
                StateMachine.add('FIND',
                                 FindPeople().find_people_,
                                 transitions={
                                     'invalid': 'META_NAV',
                                     'valid': 'FIND',
                                     'preempted': 'preempted'
                                 },
                                 remapping={'pos_xm': 'pos_xm'})
                self.meta_nav = Concurrence(
                    outcomes=['time_over', 'get_pos', 'aborted'],
                    default_outcome='aborted',
                    outcome_map={
                        'time_over': {
                            'WAIT': 'succeeded'
                        },
                        'get_pos': {
                            'NAV': 'succeeded'
                        },
                        'aborted': {
                            'NAV': 'aborted'
                        }
                    },
                    child_termination_cb=self.nav_child_cb,
                    input_keys=['pos_xm'])
                with self.meta_nav:
                    Concurrence.add('NAV',
                                    NavStack(),
                                    remapping={'pos_xm': 'pos_xm'})
                    Concurrence.add('WAIT', Wait_trace())
                StateMachine.add('META_NAV',
                                 self.meta_nav,
                                 transitions={
                                     'get_pos': 'FIND',
                                     'time_over': 'FIND',
                                     'aborted': 'FIND'
                                 })
            Concurrence.add('FOLLOW', self.meta_follow)
            Concurrence.add('STOP',
                            CheckStop(),
                            remapping={
                                'PT_LIST': 'PT_LIST',
                                'mission': 'mission'
                            })

        self.shopping = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])
        with self.shopping:

            self.shopping.userdata.PT_LIST = {}
            self.shopping.userdata.mission = {}
            self.shopping.userdata.task = list()
            self.shopping.userdata.rec = 5.0

            StateMachine.add('RUNNODE',
                             RunNode(),
                             transitions={
                                 'succeeded': 'WAIT',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('WAIT',
                             Wait(),
                             transitions={
                                 'succeeded': 'TRACE',
                                 'error': 'error'
                             },
                             remapping={'rec': 'rec'})

            StateMachine.add("TRACE",
                             self.trace,
                             transitions={
                                 'remeber': 'succeeded',
                                 'stop': 'succeeded',
                                 'aborted': 'aborted'
                             })

        intro_server = IntrospectionServer('shopping', self.shopping,
                                           'SM_ROOT')
        intro_server.start()
        out = self.shopping.execute()
        intro_server.stop()
class SMACHAI():
    def __init__(self):
        rospy.init_node('petit_smach_ai', anonymous=False)

        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)

        # Create a list to hold the target quaternions (orientations)
        quaternions = list()

        # First define the corner orientations as Euler angles
        euler_angles = (pi / 2, pi, 3 * pi / 2, 0)

        # Then convert the angles to quaternions
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)

        # Create a list to hold the waypoint poses
        self.waypoints = list()

        self.square_size = 1.0

        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
        self.waypoints.append(
            Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
        self.waypoints.append(
            Pose(Point(self.square_size, self.square_size, 0.0),
                 quaternions[1]))
        self.waypoints.append(
            Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))

        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)

        self.stopping = False
        self.recharging = False

        # State machine for Action1
        self.sm_action1 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])
        self.sm_action1.userdata.speed = 0.1
        self.sm_action1.userdata.distance = 1.0

        with self.sm_action1:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'FORWARD',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('FORWARD',
                             MoveForward(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action2
        self.sm_action2 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])
        self.sm_action2.userdata.speed = -0.1
        self.sm_action2.userdata.distance = 0.5

        with self.sm_action2:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'SIDE',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('SIDE',
                             MoveSide(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action3
        self.sm_action3 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action3:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action4
        self.sm_action4 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action4:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action5
        self.sm_action5 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action5:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Action6
        self.sm_action6 = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['waypoint_in'],
            output_keys=['waypoint_out'])

        with self.sm_action6:
            StateMachine.add('NAV_WAYPOINT',
                             Nav2Waypoint(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

# State machine for Actions
        self.sm_actions = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])
        self.sm_actions.userdata.waypoints = self.waypoints

        with self.sm_actions:
            StateMachine.add('PICK_WAYPOINT',
                             ServiceState('/PETIT/get_objective',
                                          GetObjective,
                                          response_cb=self.objective_cb,
                                          output_keys=['waypoint_out'],
                                          outcomes=[
                                              'action1', 'action2', 'action3',
                                              'action4', 'action5', 'action6',
                                              'aborted', 'succeeded',
                                              'preempted'
                                          ]),
                             transitions={
                                 'action1': 'SM_ACTION1',
                                 'action2': 'SM_ACTION2',
                                 'action3': 'SM_ACTION3',
                                 'action4': 'SM_ACTION4',
                                 'action5': 'SM_ACTION5',
                                 'action6': 'SM_ACTION6',
                                 'aborted': 'SM_ACTION1'
                             },
                             remapping={'waypoint_out': 'patrol_waypoint'})
            #StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
            #                 transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
            #                 remapping={'waypoint_out':'patrol_waypoint'})
            StateMachine.add('SM_ACTION1',
                             self.sm_action1,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION2',
                             self.sm_action2,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION3',
                             self.sm_action3,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION4',
                             self.sm_action4,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION5',
                             self.sm_action5,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('SM_ACTION6',
                             self.sm_action6,
                             transitions={
                                 'succeeded': 'REMOVE_OBJECTIVE',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'waypoint_in': 'patrol_waypoint',
                                 'waypoint_out': 'remove_waypoint'
                             })
            StateMachine.add('REMOVE_OBJECTIVE',
                             RemoveObjective(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             },
                             remapping={'waypoint_in': 'remove_waypoint'})

# State machine with concurrence
        self.sm_concurrent = Concurrence(
            outcomes=['succeeded', 'stop'],
            default_outcome='succeeded',
            child_termination_cb=self.concurrence_child_termination_cb,
            outcome_cb=self.concurrence_outcome_cb)

        # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
        with self.sm_concurrent:
            Concurrence.add('SM_ACTIONS', self.sm_actions)
            Concurrence.add(
                'MONITOR_TIME',
                MonitorState("/GENERAL/remain", Int32, self.time_cb))
            Concurrence.add('MONITOR_BATTERY',
                            MonitorState("/PETIT/adc", Int32, self.battery_cb))

        # Create the top level state machine
        self.sm_top = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        # Add nav_patrol, sm_recharge and a Stop() machine to sm_top
        with self.sm_top:
            StateMachine.add('CONCURRENT',
                             self.sm_concurrent,
                             transitions={
                                 'succeeded': 'CONCURRENT',
                                 'stop': 'STOP'
                             })
            #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})

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

    def time_cb(self, userdata, msg):
        if msg.data < 2:
            self.stopping = True
            return False
        else:
            self.stopping = False
            return True

    def battery_cb(self, userdata, msg):
        if msg.data < 320:
            self.recharging = True
            return False
        else:
            self.recharging = False
            return True

    def objective_cb(self, userdata, response):
        #objective_response = GetObjective().Response
        userdata.waypoint_out = response.goal
        waypoint_type = response.type.data

        rospy.loginfo("goal: " + str(response.goal))

        if (waypoint_type == 1):
            return 'action1'
        if (waypoint_type == 2):
            return 'action2'
        if (waypoint_type == 3):
            return 'action3'
        if (waypoint_type == 4):
            return 'action4'
        if (waypoint_type == 5):
            return 'action5'
        if (waypoint_type == 6):
            return 'action6'
        return 'aborted'

    # Gets called when ANY child state terminates
    def concurrence_child_termination_cb(self, outcome_map):
        # If the current navigation task has succeeded, return True
        if outcome_map['SM_ACTIONS'] == 'succeeded':
            return True
        # If the MonitorState state returns False (invalid), store the current nav goal and recharge
        if outcome_map['MONITOR_TIME'] == 'invalid':
            rospy.loginfo("LOW TIME! NEED TO STOP...")
            return True
        if outcome_map['MONITOR_BATTERY'] == 'invalid':
            rospy.loginfo("LOW BATTERY! NEED TO STOP...")
            return True
        else:
            return False

    # Gets called when ALL child states are terminated
    def concurrence_outcome_cb(self, outcome_map):
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['MONITOR_TIME'] == 'invalid':
            rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ")
            return 'stop'
        if outcome_map['MONITOR_BATTERY'] == 'invalid':
            return 'stop'
        # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
        elif outcome_map['SM_ACTIONS'] == 'succeeded':
            #self.patrol_count += 1
            #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
            # If we have not completed all our patrols, start again at the beginning
            #if self.n_patrols == -1 or self.patrol_count < self.n_patrols:
            #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData())
            return 'succeeded'
            # Otherwise, we are finished patrolling so return 'stop'
            #else:
            #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData())
            #return 'stop'
        # Recharge if all else fails
        else:
            return 'recharge'

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")

        self.sm_actions.request_preempt()

        self.cmd_vel_pub.publish(Twist())

        rospy.sleep(1)
Exemple #40
0
class Follow():
    def __init__(self):
        rospy.init_node('follow')
        rospy.logerr('gogogo')

        self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'],
                                 default_outcome='stop',
                                 outcome_map={
                                     'remeber': {
                                         'STOP': 'remeber'
                                     },
                                     'stop': {
                                         'STOP': 'stop'
                                     },
                                     'aborted': {
                                         'FOLLOW': 'aborted'
                                     }
                                 },
                                 child_termination_cb=self.trace_child_cb,
                                 input_keys=['PT_LIST', 'mission'],
                                 output_keys=['PT_LIST', 'mission'])
        with self.trace:
            self.meta_follow = StateMachine(
                ['succeeded', 'aborted', 'preempted'])
            with self.meta_follow:
                StateMachine.add('FIND',
                                 FindPeople().find_people_,
                                 transitions={
                                     'invalid': 'META_NAV',
                                     'valid': 'FIND',
                                     'preempted': 'preempted'
                                 },
                                 remapping={'pos_xm': 'pos_xm'})
                self.meta_nav = Concurrence(
                    outcomes=['time_over', 'get_pos', 'aborted'],
                    default_outcome='aborted',
                    outcome_map={
                        'time_over': {
                            'WAIT': 'succeeded'
                        },
                        'get_pos': {
                            'NAV': 'succeeded'
                        },
                        'aborted': {
                            'NAV': 'aborted'
                        }
                    },
                    child_termination_cb=self.nav_child_cb,
                    input_keys=['pos_xm'])
                with self.meta_nav:
                    Concurrence.add('NAV',
                                    NavStack(),
                                    remapping={'pos_xm': 'pos_xm'})
                    Concurrence.add('WAIT', Wait_trace())
                StateMachine.add('META_NAV',
                                 self.meta_nav,
                                 transitions={
                                     'get_pos': 'FIND',
                                     'time_over': 'FIND',
                                     'aborted': 'FIND'
                                 })
            Concurrence.add('FOLLOW', self.meta_follow)
            Concurrence.add('STOP',
                            CheckStop(),
                            remapping={
                                'PT_LIST': 'PT_LIST',
                                'mission': 'mission'
                            })

        self.shopping = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])
        with self.shopping:

            self.shopping.userdata.PT_LIST = {}
            self.shopping.userdata.mission = {}
            self.shopping.userdata.task = list()
            self.shopping.userdata.rec = 5.0

            StateMachine.add('RUNNODE',
                             RunNode(),
                             transitions={
                                 'succeeded': 'WAIT',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('WAIT',
                             Wait(),
                             transitions={
                                 'succeeded': 'TRACE',
                                 'error': 'error'
                             },
                             remapping={'rec': 'rec'})

            StateMachine.add("TRACE",
                             self.trace,
                             transitions={
                                 'remeber': 'succeeded',
                                 'stop': 'succeeded',
                                 'aborted': 'aborted'
                             })

        intro_server = IntrospectionServer('shopping', self.shopping,
                                           'SM_ROOT')
        intro_server.start()
        out = self.shopping.execute()
        intro_server.stop()

    def trace_child_cb(self, outcome_map):
        if outcome_map['STOP'] == 'stop':
            rospy.logwarn('get the stop signal, stop tracing ........')
            pid = get_pid("people_tracking")
            subprocess.call('kill ' + str(pid[0]), shell=True)
            return True
        elif outcome_map['STOP'] == 'remeber':
            rospy.logwarn('ready to remeber the position')
            return True

        if outcome_map['FOLLOW']:
            rospy.logerr('the follow state meet error!')
            return True
        return False

    def nav_child_cb(self, outcome_map):
        if outcome_map['WAIT'] == 'succeeded':
            rospy.logwarn('get the pos again')
            return True
        elif outcome_map['NAV'] == 'succeeded':
            return True
        elif outcome_map['NAV'] == 'aborted':
            return True
        else:
            print(outcome_map)
            return False
Exemple #41
0
    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 = '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(40.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()
Exemple #42
0
 def build_state_machine(self): 
     '''
     建立状态机
     '''
     if not self.sm_carry_object is None:
         self.sm_carry_object = None
     self.sm_carry_object = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
     with self.sm_carry_object:
         StateMachine.add('GRASP_OBJECT', GraspObject(), transitions={'succeeded':'GET_LOCATION',
                                                                            'aborted':'END_ABORT'})
         StateMachine.add('GET_LOCATION', GetLocation(), transitions={'succeeded':'NAV_TO_WAYPOINT',
                                                                            'aborted':'END_ABORT'},
                          remapping={'waypoint_out':'nav_waypoint'})
         StateMachine.add('NAV_TO_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'RELEASE_OBJECT',
                                                                            'aborted':'END_ABORT'},
                          remapping={'waypoint_in':'nav_waypoint'}) 
         StateMachine.add('RELEASE_OBJECT', ReleaseObject(), transitions={'succeeded':'TURN_BODY',
                                                                            'aborted':'END_ABORT'})
         StateMachine.add('TURN_BODY', TurnBody(), transitions={'succeeded':'GRASP_OBJECT',
                                                                            'aborted':'END_ABORT'})
         StateMachine.add('END_ABORT', EndAbort(), transitions={'succeeded':'succeeded'}) 
         StateMachine.add('END_SUCESS', EndSuccess(), transitions={'succeeded':'succeeded'}) 
Exemple #43
0
            StateMachine.add('START_POSITION', GoToPostureState(startPose, 0.5), transitions={'succeeded': 'succeeded'})

class HomeOff_SM(StateMachine):

    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'})

# TEST
if __name__ == '__main__':
    rospy.init_node('ON_OFF_TEST')
    sm = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    # Wrap command line arguments
    if len(sys.argv) == 1:
        init=None
        startPose=None
    elif len(sys.argv) == 2:
        init=str(sys.argv[1])
        startPose=None
    else:
        init=str(sys.argv[1])
        startPose=str(sys.argv[2])

    with sm:
        StateMachine.add('HOME', HomeState(init, startPose), transitions={'succeeded': 'succeeded'})
    sm.execute()
Exemple #44
0
    def __init__(self):

        rospy.init_node('ware_house', anonymous=False)
        # rospy.on_shutdown(self.shutdown)
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server...")
        self.move_base.wait_for_server(rospy.Duration(15))
        rospy.loginfo("Connected to move_base action server")
        self.nav_goal = None

        quaternions = list()
        euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)

        self.waypoints = list()
        self.waypoints.append(Pose(Point(-2.92, 11.8, 0.0025), quaternions[3]))
        self.waypoints.append(Pose(Point(-2.7, 19.7, 0.0035), quaternions[0]))
        self.waypoints.append(Pose(Point(12.6, 18.9, 0.0183), quaternions[1]))
        self.waypoints.append(Pose(Point(11.5, 7.17, 0.0121), quaternions[1]))

        room_locations = (('starting_point', self.waypoints[0]),
                          ('loading_point1', self.waypoints[1]),
                          ('loading_point2', self.waypoints[2]),
                          ('unloading_point', self.waypoints[3]))

        self.room_locations = OrderedDict(room_locations)
        nav_states = {}

        def smach(self):
            for room in self.room_locations.iterkeys():

                def nav_goal_cb(userdata, goal):
                    nav_goal = MoveBaseGoal()
                    nav_goal.target_pose.header.frame_id = 'map'
                    nav_goal.target_pose.pose = self.waypoints[room]
                    return nav_goal

                move_base_state = SimpleActionState(
                    'move_base',
                    MoveBaseAction,
                    goal=self.nav_goal,
                    goal_cb=self.nav_goal_cb,
                    exec_timeout=rospy.Duration(15.0),
                    server_wait_timeout=rospy.Duration(10.0))
                nav_states[room] = move_base_state

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

        with sm:
            StateMachine.add('POWER_ON',
                             PowerOnRobot(),
                             transitions={'succeeded': 'WAITING'})
            StateMachine.add('WAITING',
                             Waiting(),
                             transitions={'succeeded': 'STARTING'})
            StateMachine.add('STARTING',
                             nav_states['starting_point'],
                             transitions={
                                 'succeeded': 'LOADING_POINT',
                                 'aborted': 'LOADING_POINT',
                                 'preempted': 'LOADING_POINT'
                             })
            StateMachine.add('LOADING_POINT',
                             nav_states['loading_point'],
                             transitions={
                                 'succeeded': 'UNLOADING_POINT',
                                 'aborted': 'UNLOADING_POINT',
                                 'preempted': 'UNLOADING_POINT'
                             })
            StateMachine.add('UNLOADING_POINT',
                             nav_states['unloading_point'],
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

            intro_server = IntrospectionServer('restaurant', sm, '/SM_ROOT')
            intro_server.start()

            # Execute the state machine
            sm_outcome = sm.execute()
            intro_server.stop()

        def shutdown(self):
            rospy.loginfo("Stopping the robot...")
            rospy.sleep(1)
Exemple #45
0
class Navigation(Iterator):
    def __init__(self):
        Iterator.__init__(self,
                          outcomes=['succeeded', 'preempted', 'aborted'],
                          input_keys=['log_mission'],
                          it=[],
                          output_keys=['log_mission'],
                          it_label='waypoint_id',
                          exhausted_outcome='succeeded')

        self.register_start_cb(self.set_iterator_list)

        with self:
            self.sm_nav = StateMachine(
                outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
                input_keys=['waypoint_id', 'log_mission'],
                output_keys=['log_mission'])

            self.sm_nav.register_start_cb(self.start_cb_nav)
            self.sm_nav.register_termination_cb(self.termination_cb_nav)

            with self.sm_nav:
                StateMachine.add('MOVE_BASE',
                                 SimpleActionState(
                                     'move_base',
                                     MoveBaseAction,
                                     input_keys=['waypoint_id', 'log_mission'],
                                     goal_cb=self.move_base_goal_cb),
                                 transitions={
                                     'succeeded': 'SAVE_DATA',
                                     'aborted': 'continue'
                                 })

                self.save_data_service = ServiceState(
                    'make_data_acquisition',
                    RequestSaveData,
                    response_cb=self.save_data_response_cb)

                StateMachine.add('SAVE_DATA',
                                 self.save_data_service,
                                 transitions={'succeeded': 'continue'})

            # Close the sm_nav machine and add it to the iterator
            Iterator.set_contained_state('FOLLOW_PATH',
                                         self.sm_nav,
                                         loop_outcomes=['continue'])

    def set_iterator_list(self, userdata, initial_states):
        with self:
            waypoints = userdata.log_mission['waypoints']
            start_index = userdata.log_mission['patrol']['index_waypoint']
            Iterator.set_iteritems(range(start_index, len(waypoints)),
                                   'waypoint_id')

    def save_data_response_cb(self, userdata, response):
        if self.save_data_service.preempt_requested():
            self.save_data_service.service_preempt()
            return 'preempted'
        return 'succeeded'

    def move_base_goal_cb(self, ud, goal):
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'map'
        waypoint = next(wp for wp in ud.log_mission['waypoints']
                        if wp["id"] == ud.waypoint_id)

        position = Point(waypoint['x'], waypoint['y'], 0.0)
        q_angle = quaternion_from_euler(0, 0, waypoint['theta'], axes='sxyz')
        orientation = Quaternion(*q_angle)

        nav_goal.target_pose.pose.position = position
        nav_goal.target_pose.pose.orientation = orientation

        return nav_goal

    def start_cb_nav(self, userdata, initial_states):
        add_marker_rviz(
            [userdata.log_mission['waypoints'][userdata.waypoint_id]], 4)

    def termination_cb_nav(self, userdata, terminal_states, container_outcome):
        status = None
        if container_outcome == 'continue' or container_outcome == 'succeeded':
            if terminal_states == ['MOVE_BASE']:
                status = 'aborted'
                add_marker_rviz(
                    [userdata.log_mission['waypoints'][userdata.waypoint_id]],
                    3)
            else:
                status = 'succeeded'
                add_marker_rviz(
                    [userdata.log_mission['waypoints'][userdata.waypoint_id]],
                    2)
        else:
            deleteall_marker_rviz()

        pym.cycle.update_one({"_id": userdata.log_mission['_id']}, {
            "$set": {
                "data.patrol.index_waypoint": userdata.waypoint_id
            },
            "$push": {
                "data.navigation": status
            }
        })
Exemple #46
0
#     def execute(self, userdata):
#         print 'two'
#         sleep(1)
#         if (userdata.counter % 2 == 0):
#             userdata.counter = userdata.counter + 1
#             return 'visible'
#         else:
#             userdata.counter = userdata.counter + 1
#             return 'not_visible'

if __name__ == '__main__':
    rospy.init_node("behaviour_output")
    discovered = []
    rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

    sm = StateMachine(outcomes=['success', 'failure', 'out_of_tags'])
    sm.userdata.tags = []
    sm.userdata.furthest_tag_id = -1
    with sm:
        # def result(userdata, sta)
        StateMachine.add('LOOKING_FOR_FIRST_TARGET',
                         Looking_for_first_target(),
                         transitions={
                             'found': 'APPROACHING_TARGET',
                             'not_found': 'failure'
                         },
                         remapping={
                             'tags': 'tags',
                             'furthest_tag_id': 'furthest_tag_id'
                         })
        StateMachine.add('APPROACHING_TARGET',
Exemple #47
0
    def __init__(self):
        rospy.init_node('GpsrSmach')
        rospy.on_shutdown(self.shutdown)
        rospy.logerr('you are all sillyb')
        self.smach_bool = False

        # xm_arm_moveit_name('nav_pose')#最开始时,把机械臂降下来

        self.sm_EnterRoom = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])
        with self.sm_EnterRoom:
            # if use arm, use this state
            # self.sm_EnterRoom.userdata.arm_mode_1 =0
            # self.sm_EnterRoom.userdata.arm_ps = PointStamped()
            # StateMachine.add('NAV_POSE',
            #                     ArmCmd(),
            #                     transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
            #                     remapping ={'arm_ps':'arm_ps','mode':'arm_mode_1'})

            #first detect the door is open or not
            StateMachine.add('DOOR_DETECT',
                             DoorDetect().door_detect_,
                             transitions={
                                 'invalid': 'WAIT',
                                 'valid': 'DOOR_DETECT',
                                 'preempted': 'aborted'
                             })

            # after clear the costmap, wait for some seconds
            self.sm_EnterRoom.userdata.rec = 3.0
            StateMachine.add('WAIT',
                             Wait(),
                             transitions={
                                 'succeeded': 'SIMPLE_MOVE',
                                 'error': 'error'
                             },
                             remapping={'rec': 'rec'})

            # before executing the nav-stack, move directly through the door may make the nav-stack easier
            self.sm_EnterRoom.userdata.go_point = Point(0.1, 0.0, 0.0)
            StateMachine.add('SIMPLE_MOVE',
                             SimpleMove_move(),
                             remapping={'point': 'go_point'},
                             transitions={
                                 'succeeded': 'NAV_1',
                                 'aborted': 'NAV_1',
                                 'error': 'error'
                             })

            # go to the known Pose in the map to get the gpsr-command
            self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
                'pos']
            StateMachine.add('NAV_1',
                             NavStack(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'NAV_1',
                                 'error': 'error'
                             },
                             remapping={'pos_xm': 'start_waypoint'})

        self.sm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                    input_keys=['target', 'current_task'])
        # 寻找人
        with self.sm_Find:

            # for the people-finding task, pay attention that at the end of the task,
            # run a simple executable file to make the cv-node release the KinectV2
            # otherwise, the find-object task may throw error and all the cv-node may
            # break down
            self.sm_Person = StateMachine(
                outcomes=['succeeded', 'aborted', 'error'])

            with self.sm_Person:
                self.sm_Person.userdata.rec = 2.0
                # run the people_tracking node
                StateMachine.add('RUNNODE',
                                 RunNode(),
                                 transitions={
                                     'succeeded': 'WAIT',
                                     'aborted': 'succeeded'
                                 })
                StateMachine.add('WAIT',
                                 Wait(),
                                 transitions={
                                     'succeeded': 'GET_PEOPLE_POS',
                                     'error': 'error'
                                 },
                                 remapping={'rec': 'rec'})
                #the data should be PointStamped()
                # 这里必须先运行节点,也就是用RunNode()状态
                self.sm_Person.userdata.pos_xm = Pose()
                StateMachine.add('GET_PEOPLE_POS',
                                 FindPeople().find_people_,
                                 transitions={
                                     'invalid': 'NAV_PEOPLE',
                                     'valid': 'SPEAK',
                                     'preempted': 'aborted'
                                 },
                                 remapping={'pos_xm': 'pos_xm'})
                # this state will use the userdata remapping in the last state

                StateMachine.add('NAV_PEOPLE',
                                 NavStack(),
                                 transitions={
                                     'succeeded': 'SPEAK',
                                     'aborted': 'NAV_PEOPLE',
                                     'error': 'error'
                                 },
                                 remapping={'pos_xm': 'pos_xm'})
                self.sm_Person.userdata.sentences = 'I find you'
                StateMachine.add('SPEAK',
                                 Speak(),
                                 transitions={
                                     'succeeded': 'CLOSEKINECT',
                                     'error': 'error'
                                 },
                                 remapping={'sentences': 'sentences'})

                # close the KinectV2
                StateMachine.add('CLOSEKINECT',
                                 CloseKinect(),
                                 transitions={
                                     'succeeded': 'succeeded',
                                     'aborted': 'aborted'
                                 })

            self.sm_Pos = StateMachine(
                outcomes=['succeeded', 'aborted', 'error'],
                input_keys=['target', 'current_task'])
            with self.sm_Pos:
                # this command may be foolish, but it may be used in the next competition
                # get the name of the target

                # in the target_gpsr.py we define some Pose() , they are all named as ${room_name}+'_table_1(2)'
                # because each room we have some best detecting positions, we should make xm go to these places
                # for object-find tasks
                self.sm_Pos.userdata.pose = Pose()
                self.sm_Pos.userdata.mode_1 = 1
                StateMachine.add('GET_POS',
                                 GetPos(),
                                 remapping={
                                     'target': 'target',
                                     'current_task': 'current_task',
                                     'pose': 'pose',
                                     'mode': 'mode_1'
                                 },
                                 transitions={
                                     'succeeded': 'NAV_HEHE',
                                     'aborted': 'aborted',
                                     'error': 'error'
                                 })

                StateMachine.add('NAV_HEHE',
                                 NavStack(),
                                 remapping={'pos_xm': 'pose'},
                                 transitions={
                                     'succeeded': 'GET_TARGET',
                                     'aborted': 'NAV_HEHE',
                                     'error': 'error'
                                 })

                self.sm_Pos.userdata.target_mode = 0
                self.sm_Pos.userdata.name = ''
                StateMachine.add('GET_TARGET',
                                 GetTarget(),
                                 remapping={
                                     'target': 'target',
                                     'current_task': 'current_task',
                                     'mode': 'target_mode',
                                     'current_target': 'name'
                                 },
                                 transitions={
                                     'succeeded': 'FIND_OBJECT',
                                     'aborted': 'aborted',
                                     'error': 'error'
                                 })

                self.sm_Pos.userdata.object_pos = PointStamped()
                StateMachine.add('FIND_OBJECT',
                                 FindObject(),
                                 transitions={
                                     'succeeded': 'SPEAK',
                                     'aborted': 'GET_POS_1',
                                     'error': 'error'
                                 },
                                 remapping={
                                     'name': 'name',
                                     'object_pos': 'object_pos'
                                 })

                # if xm not finds the object in the first detecting-place, she should go to the second
                # specified place for detect-task, but as U can see in the target_gpsr.py, some room are
                # only one best detecting-place
                self.sm_Pos.userdata.pose_1 = Pose()
                self.sm_Pos.userdata.mode_2 = 2
                StateMachine.add('GET_POS_1',
                                 GetPos(),
                                 remapping={
                                     'target': 'target',
                                     'current_task': 'current_task',
                                     'pose': 'pose_1',
                                     'mode': 'mode_2'
                                 },
                                 transitions={
                                     'succeeded': 'NAV_HAHA',
                                     'aborted': 'aborted',
                                     'error': 'error'
                                 })

                StateMachine.add('NAV_HAHA',
                                 NavStack(),
                                 remapping={'pos_xm': 'pose_1'},
                                 transitions={
                                     'succeeded': 'FIND_OBJECT_1',
                                     'aborted': 'NAV_HAHA',
                                     'error': 'error'
                                 })

                self.sm_Pos.userdata.object_pos = PointStamped()
                StateMachine.add('FIND_OBJECT_1',
                                 FindObject(),
                                 transitions={
                                     'succeeded': 'SPEAK',
                                     'aborted': 'SPEAK',
                                     'error': 'error'
                                 },
                                 remapping={
                                     'name': 'name',
                                     'object_pos': 'object_pos'
                                 })

                self.sm_Pos.userdata.sentences = 'I find it'
                StateMachine.add('SPEAK',
                                 Speak(),
                                 transitions={
                                     'succeeded': 'succeeded',
                                     'error': 'error'
                                 },
                                 remapping={'sentences': 'sentences'})

            # this state is used for swithing the mode, finding people or finding object?
            StateMachine.add('PERSON_OR_POS',
                             PersonOrPosition(),
                             transitions={
                                 'person': 'PERSON',
                                 'position': 'POS',
                                 'error': 'error'
                             },
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task'
                             })
            StateMachine.add('PERSON',
                             self.sm_Person,
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })
            StateMachine.add('POS',
                             self.sm_Pos,
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task'
                             })

        #nav tasks
        self.sm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                   input_keys=['target', 'current_task'])
        with self.sm_Nav:
            self.sm_Nav.userdata.pos_xm = Pose()
            self.sm_Nav.userdata.target_mode = 1
            StateMachine.add('GETTARGET',
                             GetTarget(),
                             transitions={
                                 'succeeded': "NAV_GO",
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'target': 'target',
                                 'current_task': "current_task",
                                 'current_target': 'pos_xm',
                                 'mode': 'target_mode'
                             })
            # StateMachine.add('FINDWAY',
            #                     FindWay(),
            #                     transitions = {'succeeded':'NAV_PATH1','aborted':'NAV_GO','error':'NAV_GO'},
            #                     remapping={'way_path1':'way_path1','way_path2':'way_path2'})
            # StateMachine.add('NAV_PATH1',
            #                     NavStack(),
            #                     transitions={'succeeded':'NAV_PATH2','aborted':'NAV_PATH1','error':'error'},
            #                     remapping={'pos_xm':'way_path1'})
            # StateMachine.add('NAV_PATH2',
            #                     NavStack(),
            #                     transitions = {'succeeded':'NAV_GO','aborted':'NAV_PATH2','error':'error'},
            #                     remapping={'pos_xm':'way_path2'})

            StateMachine.add('NAV_GO',
                             NavStack(),
                             transitions={
                                 'succeeded': 'SPEAK',
                                 'aborted': 'NAV_GO',
                                 'error': 'error'
                             },
                             remapping={'pos_xm': 'pos_xm'})

            self.sm_Nav.userdata.sentences = 'I arrive here'
            StateMachine.add('SPEAK',
                             Speak(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'error': 'error'
                             },
                             remapping={'sentences': 'sentences'})

        #follow task
        # we will use the concurrence fun
        # the tutorials in the wiki too easy, it is no help for you to understand the concurrence
        # please see the test-snippet of the source of executive-smach
        # https://github.com/ros/executive_smach/blob/indigo-devel/smach_ros/test/concurrence.py
        self.sm_Follow = Concurrence(outcomes=['succeeded', 'aborted'],
                                     default_outcome='succeeded',
                                     outcome_map={
                                         'succeeded': {
                                             'STOP': 'stop'
                                         },
                                         'aborted': {
                                             'FOLLOW': 'aborted'
                                         }
                                     },
                                     child_termination_cb=self.child_cb)

        with self.sm_Follow:
            self.meta_follow = StateMachine(outcomes=['succeeded', 'aborted'])
            with self.meta_follow:
                StateMachine.add('FOLLOW',
                                 SimpleFollow(),
                                 transitions={
                                     'succeeded': 'FOLLOW',
                                     'aborted': 'aborted',
                                     'preempt': 'succeeded'
                                 })
            Concurrence.add('FOLLOW', self.meta_follow)
            Concurrence.add('STOP', CheckStop())
            Concurrence.add('RUNNODE', RunNode())
            # self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error'])
            # with self.meta_Follow:

            #     StateMachine.add('RUNNODE',
            #                         RunNode(),
            #                         transitions={'succeeded':'FIND_PEOPLE','aborted':'succeeded'})
            #     self.meta_Follow.userdata.pos_xm = Pose()
            #     StateMachine.add('FIND_PEOPLE',
            #                         FindPeople().find_people_,
            #                         remapping ={'pos_xm':'pos_xm'},
            #                         transitions ={'invalid':'MOVE','valid':'FIND_PEOPLE','preempted':'aborted'})
            #     StateMachine.add('MOVE',
            #                         NavStack(),
            #                         remapping ={'pos_xm':'pos_xm'},
            #                         transitions={'succeeded':'FIND_PEOPLE','aborted':'MOVE','error':'error'})
            # self.meta_Stop = StateMachine(outcomes =['succeeded','aborted'])
            # with self.meta_Stop:
            #     StateMachine.add('STOP',
            #                         StopFollow(),
            #                         transitions ={'succeeded':'succeeded','aborted':'STOP'})

            # Concurrence.add('FOLLOW',
            #                     self.meta_Follow)
            # Concurrence.add('STOP',
            #                     self.meta_Stop)

        # speak task
        self.sm_Talk = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
        with self.sm_Talk:
            self.sm_Talk.userdata.people_condition = list()
            StateMachine.add('SPEAK',
                             Anwser(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted'
                             })

        self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                    input_keys=['target', 'current_task'])

        with self.sm_Pick:

            self.sm_Pick.userdata.name = ''
            self.sm_Pick.userdata.target_mode = 0
            self.sm_Pick.userdata.objmode = -1
            StateMachine.add('RUNNODE_IMG',
                             RunNode_img(),
                             transitions={
                                 'succeeded': 'GETNAME',
                                 'aborted': 'aborted'
                             })

            StateMachine.add('GETNAME',
                             GetTarget(),
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task',
                                 'mode': 'target_mode',
                                 'current_target': 'name'
                             },
                             transitions={
                                 'succeeded': 'FIND_OBJECT',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })
            self.sm_Pick.userdata.object_pos = PointStamped()
            StateMachine.add('FIND_OBJECT',
                             FindObject(),
                             transitions={
                                 'succeeded': 'POS_JUSTFY',
                                 'aborted': 'FIND_OBJECT',
                                 'error': 'SPEAK'
                             },
                             remapping={
                                 'name': 'name',
                                 'object_pos': 'object_pos',
                                 'objmode': 'objmode'
                             })

            #making the xm foreward the object may make the grasping task easier
            self.sm_Pick.userdata.pose = Pose()
            StateMachine.add('POS_JUSTFY',
                             PosJustfy(),
                             remapping={
                                 'object_pos': 'object_pos',
                                 'pose': 'pose'
                             },
                             transitions={
                                 'succeeded': 'NAV_TO',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })
            StateMachine.add('NAV_TO',
                             NavStack(),
                             transitions={
                                 'succeeded': 'RUNNODE_IMG2',
                                 'aborted': 'NAV_TO',
                                 'error': 'error'
                             },
                             remapping={"pos_xm": 'pose'})
            StateMachine.add('RUNNODE_IMG2',
                             RunNode_img(),
                             transitions={
                                 'succeeded': 'FIND_AGAIN',
                                 'aborted': 'aborted'
                             })
            StateMachine.add('FIND_AGAIN',
                             FindObject(),
                             transitions={
                                 'succeeded': 'PICK',
                                 'aborted': 'FIND_AGAIN',
                                 'error': 'SPEAK'
                             },
                             remapping={
                                 'name': 'name',
                                 'object_pos': 'object_pos',
                                 'objmode': 'objmode'
                             })
            self.sm_Pick.userdata.arm_mode_1 = 1
            StateMachine.add('PICK',
                             ArmCmd(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'arm_ps': 'object_pos',
                                 'mode': 'arm_mode_1'
                             })
            self.sm_Pick.userdata.sentences = 'xiao meng can not find things'
            StateMachine.add('SPEAK',
                             Speak(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })

        self.sm_Place = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'])
        with self.sm_Place:
            # place_ps please specified due to the scene
            self.sm_Place.userdata.place_ps = PointStamped()
            self.sm_Place.userdata.place_ps.header.frame_id = 'base_link'
            self.sm_Place.userdata.place_ps.point.x = 0.8
            self.sm_Place.userdata.place_ps.point.y = 0.0
            self.sm_Place.userdata.place_ps.point.z = 0.6
            self.sm_Place.userdata.objmode = 2
            # without moveit, if is just place it in a open space
            self.sm_Place.userdata.arm_mode_1 = 2
            StateMachine.add('PLACE',
                             ArmCmd(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'PLACE',
                                 'error': 'error'
                             },
                             remapping={
                                 'arm_ps': 'place_ps',
                                 'mode': 'arm_mode_1'
                             })

        self.sm_GPSR = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
        with self.sm_GPSR:
            self.sm_GPSR.userdata.target = list()
            self.sm_GPSR.userdata.action = list()
            self.sm_GPSR.userdata.task_num = 0
            self.sm_GPSR.userdata.current_task = -1
            self.sm_GPSR.userdata.current_turn = 1
            self.sm_GPSR.userdata.turn = 3
            self.sm_GPSR.userdata.pos_xm_door = gpsr_target['speaker']['pos']
            self.sm_GPSR.userdata.sentences = 'please command me'
            StateMachine.add('SPEAK_RESTART',
                             Speak(),
                             transitions={
                                 'succeeded': 'RECEIVE_TASKS',
                                 'aborted': 'RECEIVE_TASKS',
                                 'error': 'RECEIVE_TASKS'
                             })
            # get the task due to the speech-node
            StateMachine.add('RECEIVE_TASKS',
                             GetTask(),
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'RECEIVE_TASKS',
                                 'error': 'error'
                             },
                             remapping={
                                 'target': 'target',
                                 'action': 'action',
                                 'task_num': 'task_num'
                             })
            #get the task in order and execute the task
            # if want to go to the door at the end of the task, please modify:
            # succeeded->succeeded --> succeeded->BYEBYE
            StateMachine.add('GET_NEXT_TASK',
                             NextDo(),
                             transitions={
                                 'succeeded': 'BACK_DOOR',
                                 'aborted': 'aborted',
                                 'error': 'error',
                                 'find': 'FIND',
                                 'go': 'GO',
                                 'follow': 'FOLLOW',
                                 'pick': 'PICK',
                                 'place': 'PLACE',
                                 'talk': 'TALK',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'action': 'action',
                                 'current_task': 'current_task',
                                 'task_num': 'task_num'
                             })
            StateMachine.add('BACK_DOOR',
                             NavStack(),
                             transitions={
                                 'succeeded': 'CHECK_TURN',
                                 'aborted': 'BACK_DOOR',
                                 'error': 'error'
                             },
                             remapping={'pos_xm': 'pos_xm_door'})
            StateMachine.add('CHECK_TURN',
                             CheckTurn(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'continue': 'SPEAK_RESTART',
                                 'error': 'error'
                             })

            # all the task-group
            StateMachine.add('GO',
                             self.sm_Nav,
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task'
                             },
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'GO'
                             })
            StateMachine.add('FIND',
                             self.sm_Find,
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task'
                             },
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'FIND'
                             })
            StateMachine.add('FOLLOW',
                             self.sm_Follow,
                             transitions={
                                 'succeeded': 'CLOSE',
                                 'aborted': 'FOLLOW'
                             })
            StateMachine.add('CLOSE',
                             CloseKinect(),
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'GET_NEXT_TASK'
                             })
            StateMachine.add('PICK',
                             self.sm_Pick,
                             remapping={
                                 'target': 'target',
                                 'current_task': 'current_task'
                             },
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'PICK'
                             })
            StateMachine.add('TALK',
                             self.sm_Talk,
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'TALK'
                             })
            StateMachine.add('PLACE',
                             self.sm_Place,
                             transitions={
                                 'succeeded': 'GET_NEXT_TASK',
                                 'aborted': 'PLACE'
                             })

            self.sm_GPSR.userdata.string = 'I finish all the three tasks, I will go out the stage'
            StateMachine.add('BYEBYE',
                             Speak(),
                             transitions={
                                 'succeeded': 'GOOUT',
                                 'error': 'error'
                             },
                             remapping={'sentences': 'string'})
            #no need
            self.sm_GPSR.userdata.waypoint = gpsr_target['end']['pos']
            StateMachine.add('GOOUT',
                             NavStack(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={"pos_xm": 'waypoint'})

        intro_server = IntrospectionServer('enter_room', self.sm_EnterRoom,
                                           '/SM_ROOT')
        intro_server.start()
        out_1 = self.sm_EnterRoom.execute()
        intro_server.stop()
        #只运行一次
        intro_server = IntrospectionServer('sm_gpsr', self.sm_GPSR, '/SM_ROOT')
        intro_server.start()
        out_2 = self.sm_GPSR.execute()
        intro_server.stop()
        self.smach_bool = True
Exemple #48
0
import rospy
from smach import StateMachine
import sys
import os

sys.path.append(os.path.join(os.path.dirname(__file__), "states"))

from wait import Wait
from explore import Explore
from go_back import GoBack
from report import Report

if __name__ == "__main__":
    rospy.init_node("fsm", anonymous=True)
    fsm = StateMachine(
        outcomes=["done", "exit"]
    )
    with fsm:
        StateMachine.add(
            "WAIT",
            Wait(),
            transitions={
                "commanded": "EXPLORE",
            },
        )
        StateMachine.add(
            "EXPLORE",
            Explore(),
            transitions={
                "finished": "GOBACK",
            },
def setup_statemachine(robot):
    state_machine = StateMachine(outcomes=['done'])
    state_machine.userdata['item_picked'] = None

    with state_machine:
        # Intro
        # StateMachine.add('START_CHALLENGE_ROBUST',
        #                  Initialize(robot),
        #                  transitions={'initialized': 'SAY_START', 'abort': 'done'})

        StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"),  # ToDo: in knowledge
                         transitions={'Done': 'SAY_START',
                                      'Aborted': 'done',
                                      'Failed': 'SAY_START'})

        StateMachine.add('SAY_START',
                         Say(robot,
                             "Let's set the table baby! If there are any chairs near the kitchen_table, please remove them",
                             block=False),
                         transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'})

        # The pre-work
        StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD',
                         NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"),
                         transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
                                      'failed': 'SAY_OPEN_FAILED'})

        StateMachine.add('SAY_OPEN_FAILED',
                         Say(robot, "I failed to open the cupboard drawer"),
                         transitions={'spoken': 'WAIT_OPEN'})

        StateMachine.add('WAIT_OPEN',
                         WaitTime(robot, 5),
                         transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'})

        StateMachine.add('SAY_OPEN_THANKS',
                         Say(robot, "Thank you darling"),
                         transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})

        # The loop
        StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
                         NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of",
                                                                 required_items),
                         transitions={'succeeded': 'PLACE_ITEM_ON_TABLE',
                                      'failed': 'CHECK_IF_WE_HAVE_IT_ALL'})

        StateMachine.add('PLACE_ITEM_ON_TABLE',
                         NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"),
                         transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL',
                                      'failed': 'WAIT'})

        StateMachine.add('WAIT',
                         WaitTime(robot, 2),
                         transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'})

        StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL',
                         CBState(check_if_we_have_it_all, cb_args=[robot]),
                         transitions={'we_have_it_all': 'SAY_END_CHALLENGE',
                                      'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})

        # Outro
        StateMachine.add('SAY_END_CHALLENGE',
                         Say(robot, "That was all folks, have a nice meal!"),
                         transitions={'spoken': 'done'})

        StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD',
                         NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"),
                         transitions={'succeeded': 'done',
                                      'failed': 'done'})

    return state_machine
Exemple #50
0
    def __init__(self):
        rospy.init_node('operating_mode', anonymous=False)

        global mb
        mb = MongoBridge()

        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)

        # Create the top level SMACH state machine
        self.sm_top = StateMachine(outcomes=['stop'])

        # Open the container
        with self.sm_top:
            # ===== SETUP State =====
            StateMachine.add('SETUP',
                             Setup(),
                             transitions={
                                 'succeeded': 'TOP_CONTROL',
                                 'preempted': 'stop',
                                 'aborted': 'stop'
                             })

            StateMachine.add('RECHARGE',
                             Recharge(),
                             transitions={
                                 'succeeded': 'SETUP',
                                 'preempted': 'stop',
                                 'aborted': 'stop'
                             })

            # ===== TOP_CONTROL State =====
            self.sm_battery_concurrence = Concurrence(
                outcomes=[
                    'restart', 'recharge', 'preempted', 'aborted', 'stop'
                ],
                default_outcome='recharge',
                child_termination_cb=self.child_termination_cb,
                outcome_cb=self.outcome_cb)

            self.sm_battery_concurrence.userdata.mode_change_infos = {
                'mode': None,
                'change_mode': False
            }

            # Open the container
            with self.sm_battery_concurrence:
                # Add states to the container
                Concurrence.add(
                    'BATTERY_MONITOR',
                    MonitorState("/battery_level", Float32,
                                 self.battery_monitor_cb))

                Concurrence.add(
                    'CHANGE_MODE_MONITOR',
                    MonitorState("/change_mode",
                                 String,
                                 self.change_mode_monitor_cb,
                                 input_keys=['mode_change_infos'],
                                 output_keys=['mode_change_infos']))

                self.operating_mode = StateMachine(
                    outcomes=['aborted', 'preempted', 'standby'],
                    input_keys=['mode_change_infos'])

                with self.operating_mode:
                    StateMachine.add(
                        'MODE_MANAGEMENT',
                        OperatingMode(),
                        transitions={'succeeded': 'MODE_MANAGEMENT'})

                Concurrence.add('OPERATING_MODE', self.operating_mode)

            StateMachine.add('TOP_CONTROL',
                             self.sm_battery_concurrence,
                             transitions={
                                 'restart': 'TOP_CONTROL',
                                 'recharge': 'RECHARGE',
                                 'aborted': 'stop',
                                 'preempted': 'stop'
                             })

        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('mode', 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()
Exemple #51
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
Exemple #52
0
class Main():
    def __init__(self):
        rospy.init_node('operating_mode', anonymous=False)

        global mb
        mb = MongoBridge()

        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)

        # Create the top level SMACH state machine
        self.sm_top = StateMachine(outcomes=['stop'])

        # Open the container
        with self.sm_top:
            # ===== SETUP State =====
            StateMachine.add('SETUP',
                             Setup(),
                             transitions={
                                 'succeeded': 'TOP_CONTROL',
                                 'preempted': 'stop',
                                 'aborted': 'stop'
                             })

            StateMachine.add('RECHARGE',
                             Recharge(),
                             transitions={
                                 'succeeded': 'SETUP',
                                 'preempted': 'stop',
                                 'aborted': 'stop'
                             })

            # ===== TOP_CONTROL State =====
            self.sm_battery_concurrence = Concurrence(
                outcomes=[
                    'restart', 'recharge', 'preempted', 'aborted', 'stop'
                ],
                default_outcome='recharge',
                child_termination_cb=self.child_termination_cb,
                outcome_cb=self.outcome_cb)

            self.sm_battery_concurrence.userdata.mode_change_infos = {
                'mode': None,
                'change_mode': False
            }

            # Open the container
            with self.sm_battery_concurrence:
                # Add states to the container
                Concurrence.add(
                    'BATTERY_MONITOR',
                    MonitorState("/battery_level", Float32,
                                 self.battery_monitor_cb))

                Concurrence.add(
                    'CHANGE_MODE_MONITOR',
                    MonitorState("/change_mode",
                                 String,
                                 self.change_mode_monitor_cb,
                                 input_keys=['mode_change_infos'],
                                 output_keys=['mode_change_infos']))

                self.operating_mode = StateMachine(
                    outcomes=['aborted', 'preempted', 'standby'],
                    input_keys=['mode_change_infos'])

                with self.operating_mode:
                    StateMachine.add(
                        'MODE_MANAGEMENT',
                        OperatingMode(),
                        transitions={'succeeded': 'MODE_MANAGEMENT'})

                Concurrence.add('OPERATING_MODE', self.operating_mode)

            StateMachine.add('TOP_CONTROL',
                             self.sm_battery_concurrence,
                             transitions={
                                 'restart': 'TOP_CONTROL',
                                 'recharge': 'RECHARGE',
                                 'aborted': 'stop',
                                 'preempted': 'stop'
                             })

        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('mode', 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 child_termination_cb(self, outcome_map):
        if outcome_map['BATTERY_MONITOR'] == 'invalid' or \
                outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \
                outcome_map['OPERATING_MODE'] == 'aborted' or \
                outcome_map['OPERATING_MODE'] == 'preempted':
            return True
        else:
            return False

    def outcome_cb(self, outcome_map):
        if outcome_map['BATTERY_MONITOR'] == 'invalid':
            return 'recharge'
        elif outcome_map['CHANGE_MODE_MONITOR'] == 'invalid' or \
                outcome_map['OPERATING_MODE'] == 'aborted' or \
                outcome_map['OPERATING_MODE'] == 'preempted':
            return 'restart'
        else:
            return 'stop'

    def battery_monitor_cb(self, userdata, msg):
        if msg.data < 20.0:
            return False
        else:
            return True

    def change_mode_monitor_cb(self, userdata, msg):
        if msg.data:
            data = json.loads(msg.data)
            if not hasattr(self, '_last_msg_stamp'
                           ) or self._last_msg_stamp != data['stamp']:
                print('Entrou!')
                mb.add_data({
                    'class_name': 'mode',
                    'item_name': 'mode',
                    'data': data
                })
                mb.send_data_and_wait()
                mode_id = pym.mode.find_one({}, {"_id": 1},
                                            sort=[("data.timestamp", order_des)
                                                  ])["_id"]
                if data.has_key(
                        'default_mode') and data['default_mode'] == True:
                    mb.add_data({
                        'class_name': 'mode',
                        'item_name': 'mode',
                        'params': {
                            'default_patrol': mode_id
                        }
                    })
                    mb.send_data_and_wait()
                userdata.mode_change_infos['mode'] = data['mode']
                userdata.mode_change_infos['mode_id'] = mode_id
                userdata.mode_change_infos['change_mode'] = True
                self._last_msg_stamp = data['stamp']
                return False
        else:
            return True

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")

        self.sm_top.request_preempt()

        rospy.sleep(1)
Exemple #53
0
    def __init__(self):
        StateMachine.__init__(self,
                              outcomes=['succeeded', 'preempted', 'aborted'])
        self.userdata.shopping_list = [
            'bouillon cubes', 'clove', 'coffe', 'muesli', 'popcorn', 'tea'
        ]
        self.executed_times = 0
        self.execute_nTimes = 6
        self.waitTime = 2.0

        with self:

            text = "I'm ready to do the shopping list."
            StateMachine.add('SAY_DO_SHOPPING_LIST',
                             SpeechState(text=text, blocking=True),
                             transitions={'succeeded': 'MOVE_RIGHT'})

            StateMachine.add('MOVE_RIGHT',
                             MoveToState(
                                 Pose2D(0.0, -DISTANCE_TO_OBJECT_RECOGNITION,
                                        0.0)),
                             transitions={'succeeded': 'LOOK_UP'})
            StateMachine.add('LOOK_UP',
                             JointAngleState(['HeadPitch'], [-0.5]),
                             transitions={'succeeded': 'WAIT1'})

            StateMachine.add('WAIT1',
                             TimeOutState(self.waitTime),
                             transitions={'succeeded': 'CONTROL_CHECK1'})

            def check_obj(ud):
                if self.executed_times < self.execute_nTimes:
                    self.executed_times += 1
                    print self.executed_times
                    return 'succeeded'
                else:
                    self.executed_times = 0
                    return 'ended'

            StateMachine.add('CONTROL_CHECK1',
                             CBState(check_obj,
                                     outcomes=['succeeded', 'ended'],
                                     input_keys=['in_list', 'in_detected'],
                                     output_keys=['out_list']),
                             transitions={
                                 'succeeded': 'CHECK_OBJECTS1',
                                 'ended': 'SAY_CROUCH'
                             },
                             remapping={
                                 'in_list': 'shopping_list',
                                 'out_list': 'shopping_list'
                             })

            StateMachine.add('CHECK_OBJECTS1',
                             ShoppingListState(),
                             transitions={
                                 'succeeded': 'CONTROL_CHECK1',
                                 'aborted': 'CHECK_OBJECTS1'
                             })

            StateMachine.add(
                'SAY_CROUCH',
                SpeechState('I will bend down to look the lower cupboard.',
                            blocking=False),
                transitions={'succeeded': 'CROUCH_POSE'})
            StateMachine.add('CROUCH_POSE',
                             GoToPostureState('Crouch', 0.5),
                             transitions={'succeeded': 'LOOK_DOWN_LEFT'})
            StateMachine.add('LOOK_DOWN_LEFT',
                             JointAngleState(['HeadPitch', 'HeadYaw'],
                                             [0.5, 0.5]),
                             transitions={'succeeded': 'WAIT2'})

            StateMachine.add('WAIT2',
                             TimeOutState(self.waitTime),
                             transitions={'succeeded': 'CONTROL_CHECK2'})

            StateMachine.add('CONTROL_CHECK2',
                             CBState(check_obj,
                                     outcomes=['succeeded', 'ended'],
                                     input_keys=['in_list', 'in_detected'],
                                     output_keys=['out_list']),
                             transitions={
                                 'succeeded': 'CHECK_OBJECTS2',
                                 'ended': 'LOOK_CENTER'
                             },
                             remapping={
                                 'in_list': 'shopping_list',
                                 'out_list': 'shopping_list'
                             })

            StateMachine.add('CHECK_OBJECTS2',
                             ShoppingListState(),
                             transitions={
                                 'succeeded': 'CONTROL_CHECK2',
                                 'aborted': 'CHECK_OBJECTS2'
                             })

            StateMachine.add('LOOK_CENTER',
                             JointAngleState(['HeadPitch', 'HeadYaw'],
                                             [0.0, 0.0]),
                             transitions={'succeeded': 'SAY_GOING_LEFT'})
            StateMachine.add('SAY_GOING_LEFT',
                             SpeechState(
                                 'I will go to look to the upper cupboard.',
                                 blocking=False),
                             transitions={'succeeded': 'MOVE_LEFT'})
            StateMachine.add('MOVE_LEFT',
                             MoveToState(
                                 Pose2D(0.0,
                                        2 * DISTANCE_TO_OBJECT_RECOGNITION,
                                        0.0)),
                             transitions={'succeeded': 'LOOK_UP2'})
            StateMachine.add('LOOK_UP2',
                             JointAngleState(['HeadPitch'], [-0.5]),
                             transitions={'succeeded': 'WAIT3'})

            StateMachine.add('WAIT3',
                             TimeOutState(self.waitTime),
                             transitions={'succeeded': 'CONTROL_CHECK3'})

            StateMachine.add('CONTROL_CHECK3',
                             CBState(check_obj,
                                     outcomes=['succeeded', 'ended'],
                                     input_keys=['in_list', 'in_detected'],
                                     output_keys=['out_list']),
                             transitions={
                                 'succeeded': 'CHECK_OBJECTS3',
                                 'ended': 'LOOK_FRONT'
                             },
                             remapping={
                                 'in_list': 'shopping_list',
                                 'out_list': 'shopping_list'
                             })

            StateMachine.add('CHECK_OBJECTS3',
                             ShoppingListState(),
                             transitions={
                                 'succeeded': 'CONTROL_CHECK3',
                                 'aborted': 'CHECK_OBJECTS3'
                             })

            StateMachine.add('LOOK_FRONT',
                             JointAngleState(['HeadPitch', 'HeadYaw'],
                                             [0.0, 0.0]),
                             transitions={'succeeded': 'PREPARE_TEXT'})

            def prep_text(ud):
                text_pre = "I've to buy "
                if len(self.userdata.shopping_list) == 1:
                    ud.out_text = text_pre + str(self.userdata.shopping_list)
                elif len(self.userdata.shopping_list) == 0:
                    ud.out_text = "I don't have to buy anything"
                else:
                    ud.out_text = text_pre + str(
                        self.userdata.
                        shopping_list[:len(self.userdata.shopping_list) - 1]
                    ) + ' and ' + self.userdata.shopping_list[-1]
                return 'succeeded'

            StateMachine.add(
                'PREPARE_TEXT',
                CBState(prep_text,
                        outcomes=['succeeded'],
                        input_keys=['shopping_list'],
                        output_keys=['out_text']),
                transitions={'succeeded': 'SAY_SHOPPING_LIST_DONE1'},
                remapping={'out_text': 'text'})

            StateMachine.add('SAY_SHOPPING_LIST_DONE1',
                             SpeechState(text=None, blocking=True),
                             transitions={'succeeded': 'WAIT4'})
            StateMachine.add(
                'WAIT4',
                TimeOutState(1.0),
                transitions={'succeeded': 'SAY_SHOPPING_LIST_DONE2'})
            StateMachine.add('SAY_SHOPPING_LIST_DONE2',
                             SpeechState(text=None, blocking=True),
                             transitions={'succeeded': 'WAIT5'})

            StateMachine.add('WAIT5',
                             TimeOutState(1.0),
                             transitions={'succeeded': 'SAY_FINISH'})

            text = 'I am done. Sending the shopping list to AMAZON.'
            StateMachine.add('SAY_FINISH',
                             SpeechState(text=text, blocking=False),
                             transitions={'succeeded': 'succeeded'})
Exemple #54
0
def transition_to(name, target_state):
    StateMachine.add(name, states.NoOp(), transitions={"succeeded": target_state})
Exemple #55
0
    #if rospy.Time.now() > state_change_time:
    while rospy.Time.now() < state_change_time:
      twist = Twist()
      twist.angular.z = 1
      twist.linear.x = 0
      cmd_vel_pub.publish(twist)

    state_change_time = rospy.Time.now() + rospy.Duration(30)
    return 'success'

if __name__ == '__main__':

  g_range_ahead = 1 # anything to start
  scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
  cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
  rospy.init_node('wanderSM')
  state_change_time = rospy.Time.now()
  #driving_forward = True
  rate = rospy.Rate(10)

  sm = StateMachine(outcomes=['success'])

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

  with sm:
    StateMachine.add('Forward', Forward(), transitions={'success':'Spin'})
    StateMachine.add('Spin', Spin(), transitions={'success':'Forward'})


  outcome = sm.execute()
def location3(
        cam_pixel_to_point):  # type: (CamPixelToPointServer) -> StateMachine
    sm = StateMachine(outcomes=['ok'], input_keys=['green_shape'])
    with sm:
        StateMachine.add('MOVETO1',
                         move_to_stop_line(lower=True),
                         transitions={'ok': 'TURN1_1'})
        StateMachine.add('TURN1_1',
                         RotateState(np.pi / 2),
                         transitions={'ok': 'LOCATION3_1'})
        StateMachine.add('LOCATION3_1',
                         Location3State(cam_pixel_to_point),
                         transitions={
                             'ok': 'TURN1_2',
                             'err': 'TURN1_2',
                             'match': 'EXIT_TURN1'
                         })
        StateMachine.add('TURN1_2',
                         RotateState(-np.pi / 2),
                         transitions={'ok': 'MOVETO2'})

        StateMachine.add('MOVETO2',
                         move_to_stop_line(lower=True),
                         transitions={'ok': 'TURN2_1'})
        StateMachine.add('TURN2_1',
                         RotateState(np.pi / 2),
                         transitions={'ok': 'LOCATION3_2'})
        StateMachine.add('LOCATION3_2',
                         Location3State(cam_pixel_to_point),
                         transitions={
                             'ok': 'TURN2_2',
                             'err': 'TURN2_2',
                             'match': 'EXIT_TURN2'
                         })
        StateMachine.add('TURN2_2',
                         RotateState(-np.pi / 2),
                         transitions={'ok': 'MOVETO3'})

        StateMachine.add('MOVETO3',
                         move_to_stop_line(lower=True),
                         transitions={'ok': 'TURN3_1'})
        StateMachine.add('TURN3_1',
                         RotateState(np.pi / 2),
                         transitions={'ok': 'LOCATION3_3'})
        StateMachine.add('LOCATION3_3',
                         Location3State(cam_pixel_to_point),
                         transitions={
                             'ok': 'TURN3_2',
                             'err': 'TURN3_2',
                             'match': 'EXIT_TURN3'
                         })
        StateMachine.add('TURN3_2', RotateState(-np.pi / 2))

        StateMachine.add('EXIT_TURN1',
                         RotateState(-np.pi / 2),
                         transitions={'ok': 'EXIT_MOVETO2'})
        StateMachine.add('EXIT_TURN2',
                         RotateState(-np.pi / 2),
                         transitions={'ok': 'EXIT_MOVETO3'})
        StateMachine.add('EXIT_TURN3', RotateState(-np.pi / 2))
        StateMachine.add('EXIT_MOVETO2',
                         move_to_stop_line(lower=True),
                         transitions={'ok': 'EXIT_MOVETO3'})
        StateMachine.add('EXIT_MOVETO3', move_to_stop_line(lower=True))
    return sm
Exemple #57
0
        if (mc.detach_objects(self.object_name) == 'succeeded'):
            if (hc.release()): return 'succeeded'
            else: return 'aborted'
        else: return 'aborted'


#==================================
#==================================
if __name__ == '__main__':
    rospy.init_node('scenario_node')

    na = NaviAction()
    hc = HandController()
    mc = MoveitCommand()

    go_to_shelf = StateMachine(outcomes=['succeeded', 'aborted'])
    with go_to_shelf:
        StateMachine.add('DOWN LIFTER', MOVE_LIFTER(0,0.6),\
          transitions={'succeeded':'MOVE','aborted':'aborted'})
        StateMachine.add('MOVE', GO_TO_PLACE(1),\
          transitions={'succeeded':'succeeded','aborted':'aborted'})

    go_to_start_point = StateMachine(outcomes=['succeeded', 'aborted'])
    with go_to_start_point:
        StateMachine.add('DOWN LIFTER', MOVE_LIFTER(0,0.6),\
          transitions={'succeeded':'MOVE','aborted':'aborted'})
        StateMachine.add('MOVE', GO_TO_PLACE(0),\
          transitions={'succeeded':'succeeded','aborted':'aborted'})

    pick_place_1 = StateMachine(outcomes=['succeeded', 'aborted'])
    x = mc.box1.pose.position.x
Exemple #58
0
    def __init__(self, get_one=True, ask_for_repetition=True, timeout=30):
        ''' If get_one is True, recognition_result is just the topic information. If it's false is a string with the most confident word.
            If ask_for_repetition is True, the robot asks the user to repeat the response (for now it does it forever)
        '''
        StateMachine.__init__(self,
                              outcomes=['succeeded', 'aborted', 'preempted'],
                              output_keys=['recognition_result'])
        self.userdata.recognition_result = None
        with self:
            StateMachine.add('START_LISTEN',
                             StartRecognitionState(),
                             transitions={'succeeded': 'ANSWER_DETECTION'})

            StateMachine.add(
                'ANSWER_DETECTION',
                GetRecognizedWordNoEmptyList(timeout=timeout),
                transitions={
                    'succeeded': 'STOP_LISTEN',
                    'timeouted': 'STOP_LISTEN_ABORTED',
                    'preempted': 'preempted'
                },
                remapping={'recognized_words': 'recognition_result'})

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

            def get_most_confident_word(ud):
                sorted_result = sorted(zip(
                    ud.in_recognition_result.confidence_values,
                    ud.in_recognition_result.words),
                                       reverse=True)
                ud.out_recognition_result = sorted_result[0][
                    1]  # Get the most confident [0] word [1]
                rospy.loginfo('---Get user answer recognized word: ' +
                              sorted_result[0][1])
                return 'succeeded'

            StateMachine.add('GET_SINGLE_RESPONSE',
                             CBState(get_most_confident_word,
                                     input_keys=['in_recognition_result'],
                                     output_keys=['out_recognition_result'],
                                     outcomes=['succeeded']),
                             remapping={
                                 'in_recognition_result': 'recognition_result',
                                 'out_recognition_result': 'recognition_result'
                             },
                             transitions={'succeeded': 'succeeded'})

            StateMachine.add(
                'STOP_LISTEN_ABORTED',
                StopRecognitionState(),
                transitions={
                    'succeeded':
                    'aborted' if not ask_for_repetition else 'ASK_TO_REPEAT'
                })

            repeat_pool = [
                'I could not hear you. Repeat it please.',
                'Did you say anything? Please repeat it.',
                'I did not understand anything. Answer my question.',
                'Please, answer my question.'
            ]
            StateMachine.add('ASK_TO_REPEAT',
                             SpeechFromPoolSM(pool=repeat_pool, blocking=True),
                             transitions={'succeeded': 'START_LISTEN'})
    def __init__(self):
        rospy.init_node('smach_home_status', anonymous=False)

        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)

        #####################################
        # JO IS AWAKE
        #####################################

        # State machine for Jo-awake-go-sleep
        self.sm_jo_awake_sleep = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_awake_sleep:
            StateMachine.add('LOOK_WAKE',
                             MonitorState("/JO/sleep", Empty, self.empty_cb),
                             transitions={
                                 'valid': 'GOING_SLEEP',
                                 'preempted': 'preempted',
                                 'invalid': 'GOING_SLEEP'
                             })
            StateMachine.add('GOING_SLEEP',
                             JoGoingSleep(),
                             transitions={'succeeded': 'LOOK_IN_BED'})
            StateMachine.add('LOOK_IN_BED',
                             MonitorState("/myo_disconnected", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'IN_BED',
                                 'preempted': 'preempted',
                                 'invalid': 'IN_BED'
                             })
            StateMachine.add('IN_BED',
                             JoInBed(),
                             transitions={'succeeded': 'succeeded'})

        # State machine for Jo-awake-bothgo-sleep
        self.sm_jo_awake_bothsleep = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_awake_bothsleep:
            StateMachine.add('LOOK_WAKE',
                             MonitorState("/BOTH/sleep", Empty, self.empty_cb),
                             transitions={
                                 'valid': 'GOING_SLEEP',
                                 'preempted': 'preempted',
                                 'invalid': 'GOING_SLEEP'
                             })
            StateMachine.add('GOING_SLEEP',
                             BothGoingSleep(),
                             transitions={'succeeded': 'LOOK_IN_BED'})
            StateMachine.add('LOOK_IN_BED',
                             MonitorState("/myo_disconnected", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'IN_BED',
                                 'preempted': 'preempted',
                                 'invalid': 'IN_BED'
                             })
            StateMachine.add('IN_BED',
                             BothInBed(),
                             transitions={'succeeded': 'succeeded'})

        # State machine for Jo-awake-go-out
        self.sm_jo_awake_out = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_awake_out:
            StateMachine.add('LOOK_OUT',
                             MonitorState("/JO/go_out", Empty, self.empty_cb),
                             transitions={
                                 'valid': 'PAUSE',
                                 'preempted': 'preempted',
                                 'invalid': 'PAUSE'
                             })
            StateMachine.add('PAUSE',
                             Pause(),
                             transitions={'succeeded': 'succeeded'})

        # State machine for Jo-awake
        self.sm_jo_awake = Concurrence(
            outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'],
            default_outcome='succeeded',
            child_termination_cb=self.jo_awake_child_termination_cb,
            outcome_cb=self.jo_awake_outcome_cb)

        with self.sm_jo_awake:
            Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep)
            Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep)
            Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out)

#####################################
# JO IS SLEEPING
#####################################

# State machine for Jo-sleep-waking
        self.sm_jo_sleep_waking = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_sleep_waking:
            StateMachine.add('WAIT_WAKE_UP',
                             MonitorState("/JO/wake_up", Empty, self.empty_cb),
                             transitions={
                                 'valid': 'WAKING_UP',
                                 'preempted': 'preempted',
                                 'invalid': 'WAKING_UP'
                             })
            StateMachine.add('WAKING_UP',
                             JoWakingUp(),
                             transitions={'succeeded': 'succeeded'})

        # State machine for Jo-sleep-bothwaking
        self.sm_jo_sleep_bothwaking = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_sleep_bothwaking:
            StateMachine.add('WAIT_WAKE_UP',
                             MonitorState("/BOTH/wake_up", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'WAKING_UP',
                                 'preempted': 'preempted',
                                 'invalid': 'WAKING_UP'
                             })
            StateMachine.add('WAKING_UP',
                             BothWakingUp(),
                             transitions={'succeeded': 'succeeded'})

# State machine for Jo-awake
        self.sm_jo_sleep = Concurrence(
            outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'],
            default_outcome='succeeded',
            child_termination_cb=self.jo_sleep_child_termination_cb,
            outcome_cb=self.jo_sleep_outcome_cb)

        with self.sm_jo_sleep:
            Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking)
            Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking)

#####################################
# JO IS OUT         TODO
#####################################

# State machine for Jo-out-back
        self.sm_jo_out_back = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_out_back:
            StateMachine.add('WAIT_BACK_HOME',
                             MonitorState("/JO/back_home", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'WAIT_MYO',
                                 'preempted': 'preempted',
                                 'invalid': 'WAIT_MYO'
                             })
            StateMachine.add('WAIT_MYO',
                             MonitorState("/myo_connected", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'COMING_BACK',
                                 'preempted': 'preempted',
                                 'invalid': 'COMING_BACK'
                             })
            StateMachine.add('COMING_BACK',
                             JoBackHome(),
                             transitions={'succeeded': 'succeeded'})

        # State machine for Jo-out-bothback
        self.sm_jo_out_bothback = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_jo_out_bothback:
            StateMachine.add('WAIT_BACK_HOME',
                             MonitorState("/BOTH/back_home", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'WAIT_MYO',
                                 'preempted': 'preempted',
                                 'invalid': 'WAIT_MYO'
                             })
            StateMachine.add('WAIT_MYO',
                             MonitorState("/myo_connected", Empty,
                                          self.empty_cb),
                             transitions={
                                 'valid': 'COMING_BACK',
                                 'preempted': 'preempted',
                                 'invalid': 'COMING_BACK'
                             })
            StateMachine.add('COMING_BACK',
                             BothBackHome(),
                             transitions={'succeeded': 'succeeded'})

# State machine for Jo-out
        self.sm_jo_out = Concurrence(
            outcomes=['succeeded', 'aborted', 'preempted', 'back_home'],
            default_outcome='succeeded',
            child_termination_cb=self.jo_out_child_termination_cb,
            outcome_cb=self.jo_out_outcome_cb)

        with self.sm_jo_out:
            Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back)
            Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback)

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

# 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()
Exemple #60
0
    polygon = StateMachine(outcomes=['success'])
    with polygon:
        # Add all but the final side
        for i in xrange(sides - 1):        
            StateMachine.add('SIDE_{0}'.format(i + 1),
                             Drive(1),
                             transitions={'success':'TURN_{0}'.format(i + 1)})
        # Add all the turns
        for i in xrange(sides - 1):
            StateMachine.add('TURN_{0}'.format(i + 1),
                             Turn(360.0 / sides),
                             transitions={'success':'SIDE_{0}'.format(i + 2)})
            
        # Add the final side
        StateMachine.add('SIDE_{0}'.format(sides),
                         Drive(1),
                         transitions={'success':'success'})

    return polygon

if __name__ == '__main__':
    triangle = polygon(3)
    square = polygon(4)

    shapes = StateMachine(outcomes=['success'])
    with shapes:
        StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'})
        StateMachine.add('SQUARE', square, transitions={'success':'success'})

    shapes.execute()