Пример #1
0
def explorationWithVictims():
    def _termination_cb(outcome_map):
        return True

    cc = Concurrence(outcomes=['thermal_alert', 'camera_alert', 'preempted'],
                     default_outcome='preempted',
                     outcome_map={
                         'thermal_alert': {
                             'VICTIM_MONITOR': 'thermal'
                         },
                         'camera_alert': {
                             'VICTIM_MONITOR': 'camera'
                         },
                         'preempted': {
                             'EXPLORE': 'preempted',
                             'VICTIM_MONITOR': 'preempted'
                         }
                     },
                     child_termination_cb=_termination_cb)

    with cc:

        Concurrence.add('VICTIM_MONITOR', MonitorVictimState())

        Concurrence.add('EXPLORE', simpleExplorationContainer())

    return cc
Пример #2
0
def ExplorationContainer():
	cc = Concurrence(outcomes=['victim_thermal','victim_camera','aborted','preempted','time_out'], 
	default_outcome='aborted', outcome_map={'victim_thermal':{'VICTIM_MONITOR':'victim_thermal'}, 
	'victim_camera':{'VICTIM_MONITOR':'victim_camera'},'preempted':{'EXPLORE':'preempted','VICTIM_MONITOR':'preempted'}, 
	'aborted':{'EXPLORE':'aborted'}, 'time_out':{'EXPLORE':'time_out'}},
	child_termination_cb=_termination_cb)
	
	with cc:
		#~ Concurrence.add('TARGET_CONTROLLER', utils.TargetSelectorContainer('explore'))
		Concurrence.add('EXPLORE', utils.make_iterator(utils.TargetSelectorContainer('explore'), max_iter=100))
		
		sm_victim_monitor = StateMachine(outcomes=['victim_thermal','victim_camera','preempted'])
		sm_victim_monitor.userdata.victim_type = 0
		with sm_victim_monitor:
			
			StateMachine.add('VICTIM_MONITORING', MonitorVictimState(
			input_keys=['victim_type'], output_keys=['victim_type']), 
			transitions={'invalid':'VICTIM_DECIDE', 'valid':'VICTIM_MONITORING', 'preempted':'preempted'}, 
			remapping={'victim_type':'victim_type'})
			
			StateMachine.add('VICTIM_DECIDE', DecideVictimState(), 
			transitions={'thermal':'victim_thermal','camera':'victim_camera'})
			
		Concurrence.add('VICTIM_MONITOR', sm_victim_monitor)
		
	return cc
def main():
    rospy.init_node('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):
        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'}
                            )
Пример #5
0
def main():
    rospy.init_node('smach_usecase_executive')

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

    with sm_root:
        smach.StateMachine.add('RESET',
                               ServiceState('reset', std_srvs.srv.Empty),
                               {'succeeded': 'SPAWN'})

        request = turtlesim.srv.SpawnRequest(0.0, 0.0, 0.0, 'turtle2')
        smach.StateMachine.add(
            'SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request),
            {'succeeded': 'TELEPORT1'})

        teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0, 1.0, 0.0)
        smach.StateMachine.add(
            'TELEPORT1',
            ServiceState('turtle1/teleport_absolute',
                         turtlesim.srv.TeleportAbsolute, teleport1),
            {'succeeded': 'TELEPORT2'})

        teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0, 5.0, 0.0)
        smach.StateMachine.add(
            'TELEPORT2',
            ServiceState('turtle2/teleport_absolute',
                         turtlesim.srv.TeleportAbsolute, teleport2),
            {'succeeded': 'SHAPES'})

        shapes_concurrence = Concurrence(
            outcomes=['succeeded', 'aborted', 'preempted'],
            default_outcome='aborted',
            outcome_map={
                'succeeded': {
                    'BIG': 'succeeded',
                    'SMALL': 'succeeded'
                }
            })
        smach.StateMachine.add('SHAPES', shapes_concurrence)
        with shapes_concurrence:
            Concurrence.add(
                'BIG',
                SimpleActionState('turtle_shape1', ShapeAction,
                                  ShapeGoal(11, 4.0)), {'succeeded': 'SMALL'})
            Concurrence.add(
                'SMALL',
                SimpleActionState('turtle_shape2', ShapeAction,
                                  ShapeGoal(6, 0.5)))

    sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
    sis.start()

    outcome = sm_root.execute()

    rospy.spin()
    sis.stop()
Пример #6
0
def main():
    smach_states = rospy.get_param("smach_states")
    # The autonomy_sm handles the full autonomy sequence for the competition run
    autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
    with autonomy_sm:
        # TODO: create localization state. might just be a MonitorState

        # Sequentially add all the states from the config file to the state machine,
        # where state i transitions to state i+1
        names = []
        for i in range(len(smach_states)):
            state = smach_states[i]
            name, action_state = create_action_state(state)

            # If the state name is already in the state machine, add the new
            # one with increasing numbers
            if name in names:
                name = name+"2"
                while name in names:
                    name = name[:-1]+str(int(name[-1])+1) # increase num
            names.append(name)

            StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])

        # StateMachine.add()

    # Create the concurrence contained for the fully autonomy sequence. This
    # runs the state machine for the competition run.  It also concurrently runs
    # a state with a timer counting down from 10 minutes and a state that listens
    # to the /click/start_button topic. If either of these are triggered, it will
    # end autonomy and place us into the teleop state.
    # TODO: add 10 minute competition timer state
    autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
                            default_outcome='enter_teleop',
                            child_termination_cb=autonomy_child_term_cb,
                            outcome_cb=autonomy_out_cb)
    with autonomy_concurrence:
        # state that runs full autonomy state machine
        Concurrence.add('AUTONOMY', autonomy_sm)
        # state that listens for toggle message
        Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))

    # Top level state machine, containing the autonomy and teleop machines.
    top_sm = StateMachine(outcomes=['DONE'])
    with top_sm:
        StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
        StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
          transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})

        #StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
        #  transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})

    sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
    sis.start()
    top_sm.execute()
    rospy.spin()
    sis.stop()
Пример #7
0
def dataFusionHold():
    def _termination_cb(outcome_map):
        return True

    sm = StateMachine(outcomes=['verified', 'not_verified', 'preempted'],
                      input_keys=['victim_info'],
                      output_keys=['victim_info'])

    with sm:

        #~ sm.userdata.victim_info = None

        cc = Concurrence(outcomes=['verified', 'time_out', 'preempted'],
                         default_outcome='time_out',
                         outcome_map={
                             'verified': {
                                 'MONITOR_VERIFICATION': 'got_verification'
                             },
                             'time_out': {
                                 'MONITOR_VERIFICATION': 'preempted',
                                 'TIMER': 'time_out'
                             },
                             'preempted': {
                                 'TIMER': 'preempted',
                                 'MONITOR_VERIFICATION': 'preempted'
                             }
                         },
                         child_termination_cb=_termination_cb,
                         input_keys=['victim_info'],
                         output_keys=['victim_info'])

        with cc:

            Concurrence.add('MONITOR_VERIFICATION',
                            VictimVerificationState(),
                            remapping={'victim_info': 'victim_info'})

            Concurrence.add('TIMER', DataFusionHold())

        StateMachine.add('WAIT_FOR_DF',
                         cc,
                         transitions={
                             'time_out': 'DELETE_CURRENT_HOLE',
                             'verified': 'verified',
                             'preempted': 'preempted'
                         },
                         remapping={'victim_info': 'victim_info'})

        StateMachine.add('DELETE_CURRENT_HOLE',
                         ValidateHoleState(False),
                         transitions={
                             'succeeded': 'not_verified',
                             'preempted': 'preempted'
                         })

    return sm
Пример #8
0
class sm_Follow():
    def __init__(self):
        rospy.init_node('sm_Follow')
        self.sm_FollowMe = Concurrence(
            outcomes=['succeeded', 'aborted', 'error'],
            default_outcome='succeeded',
            outcome_map={'succeeded': {
                'STOP': 'succeeded'
            }},
            child_termination_cb=self.child_cb)
        # if one state complete, the Concurrence will give a preempted signal, and will stop the current state in the preempt outcomes
        with self.sm_FollowMe:
            self.meta_Follow = StateMachine(
                outcomes=['succeeded', 'aborted', 'error'])
            with self.meta_Follow:
                self.meta_Follow.userdata.pos_xm = Pose()
                StateMachine.add('RUNNODE',
                                 RunNode(),
                                 transitions={
                                     'succeeded': 'FIND_PEOPLE',
                                     'aborted': 'aborted'
                                 })
                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)
        self.sm_FollowMe.execute()

    def child_cb(self, outcome_map):
        if outcome_map['STOP'] == 'succeeded':
            rospy.logerr('f**k')
            subprocess.call("/home/xiong/Recognition/kinect2/terminate_people",
                            shell=True)
            return True
Пример #9
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()
Пример #10
0
    def __init__(self):
        rospy.init_node('final_project_kl', anonymous=False)

        rospy.on_shutdown(self.shutdown)
        self.keyPointManager = KeyPointManager()

        # Create the nav_patrol state machine using a Concurrence container
        self.nav_patrol = Concurrence(
            outcomes=['succeeded', 'key_points', 'stop'],
            default_outcome='succeeded',
            outcome_map={'key_points': {
                'MONITOR_AR': 'invalid'
            }},
            child_termination_cb=self.concurrence_child_termination_cb,
            outcome_cb=self.concurrence_outcome_cb)
        # Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
        with self.nav_patrol:
            Concurrence.add('SM_NAV', Patrol().getSM())
            Concurrence.add(
                'MONITOR_AR',
                MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))

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

        with self.sm_top:
            StateMachine.add('PATROL',
                             self.nav_patrol,
                             transitions={
                                 'succeeded': 'PATROL',
                                 'key_points': 'PATROL_KEYPOINTS',
                                 'stop': 'STOP'
                             })
            StateMachine.add('PATROL_KEYPOINTS',
                             PatrolThroughKeyPoints(
                                 self.keyPointManager).getSM(),
                             transitions={'succeeded': 'STOP'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})

        intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
        intro_server.start()

        # Execute the state machine
        sm_outcome = self.sm_top.execute()

        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))

        intro_server.stop()
Пример #11
0
    def __init__(self):
        rospy.init_node('sm_Follow')

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

        out = self.trace.execute()
        print out
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()
Пример #13
0
class SmachHaha():
    def __init__(self):
        rospy.init_node('smach_haha')
        rospy.on_shutdown(self.shutdown)
        self.smach_top = Concurrence(
            outcomes=['succeeded', 'aborted', 'preempted'],
            default_outcome='succeeded',
            outcome_map={'succeeded': {
                'STATE_1': 'succeeded'
            }},
            child_termination_cb=self.child_termination_cb)
        with self.smach_top:
            self.sm_state2 = StateMachine(outcomes=['succeeded'])
            with self.sm_state2:
                StateMachine.add('FIRST',
                                 State2(),
                                 transitions={'succeeded': 'SECOND'})
                StateMachine.add('SECOND',
                                 State1(),
                                 transitions={'succeeded': 'FIRST'})
            Concurrence.add('STATE_1', State1())
            Concurrence.add('STATE_2', self.sm_state2)
        haha = self.smach_top.execute()

    def child_termination_cb(self, outcome_map):
        if outcome_map['STATE_1'] == 'succeeded':
            rospy.logerr('f**k you')
            return True

    def people_cb(self, UserData, msg):
        return False

    def shutdown(self):
        rospy.logerr('smach test over')
Пример #14
0
def make_nav_sm(nav_state, tennis_topic):
    """
    Make a NAV<x> state.

    :param nav_state: the state

    nav_state takes in ud called goal_position and waypoints
    it has two outcomes: goal, fail, preempted
    preempt is called when tennis is found, make sure to implement it

    goal_position is a pose (for added laziness in move_base)

    :return: the state machine (actually a Concurrence but it doesn't matter)
    """
    def i_dont_care_callback(_):
        return True  # whichever finishes first! (although tennis never finishes unless preempt...)

    # noinspection PyTypeChecker
    sm = Concurrence(outcomes=["tennis", "goal", "fail", "preempted"],
                     default_outcome="fail",
                     input_keys=["goal_position", "waypoints"],
                     output_keys=["tennis_position"],
                     outcome_map={
                         "tennis": {
                             "TENNIS_MONITOR": "tennis"
                         },
                         "fail": {
                             "NAV": "fail",
                             "TENNIS_MONITOR": "fail"
                         },
                         "goal": {
                             "NAV": "goal"
                         },
                         "preempted": {
                             "NAV": "preempted",
                             "TENNIS_MONITOR": "fail"
                         }
                     },
                     child_termination_cb=i_dont_care_callback)

    with sm:
        Concurrence.add("TENNIS_MONITOR",
                        TennisBallMonitoringState(tennis_topic),
                        remapping={"tennis_position": "tennis_position"})
        Concurrence.add("NAV", nav_state)

    return sm
Пример #15
0
def main():
    rospy.init_node('smach_usecase_executive')

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

    with sm_root:
        smach.StateMachine.add('RESET', 
                               ServiceState('reset', std_srvs.srv.Empty), 
                               {'succeeded':'SPAWN'})
        
        request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')
        smach.StateMachine.add('SPAWN', 
                               ServiceState('spawn', turtlesim.srv.Spawn, request), 
                               {'succeeded':'TELEPORT1'})
        
        teleport1 = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)
        smach.StateMachine.add('TELEPORT1', 
                               ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, teleport1), 
                                {'succeeded':'TELEPORT2'})
        
        teleport2 = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)        
        smach.StateMachine.add('TELEPORT2', 
                               ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, teleport2),
                                {'succeeded':'SHAPES'})
                               
        shapes_concurrence = Concurrence(
                            outcomes=['succeeded','aborted','preempted'],
                            default_outcome='aborted',
                            outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
        smach.StateMachine.add('SHAPES', shapes_concurrence)
        with shapes_concurrence:             
            Concurrence.add('BIG',
                               SimpleActionState('turtle_shape1', ShapeAction, ShapeGoal(11,4.0)),
                                {'succeeded':'SMALL'})
            Concurrence.add('SMALL',
                               SimpleActionState('turtle_shape2', ShapeAction, ShapeGoal(6,0.5)))
                               
                               
                               
    
    sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/Intro')
    sis.start()

    outcome = sm_root.execute()

    rospy.spin()
    sis.stop()
Пример #16
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()
Пример #17
0
def build_concurrence_container(outcomes, default_outcome, outcome_map,
                                concurrent_states):
    """Create & returns a SMACH Concurrence instance from a dictionary of SMACH states
    Args:
        outcomes            (list of str)
        default_outcome     (str)
        outcome_map         (dict of dict)
        concurrent_states   (dict of class instances)
    Returns:
        dict: A dict with instance names as keys and class instances as values
    """
    concurrence = Concurrence(outcomes=outcomes,
                              default_outcome=default_outcome,
                              outcome_map=outcome_map)
    with concurrence:
        for name, instance in concurrent_states.items():
            Concurrence.add(name.upper(), instance)
    return concurrence
Пример #18
0
    def __init__(self, distToHuman=0.9, state_machine_name="restaurant"):
        smach.Concurrence.__init__(
            self,
            outcomes=[succeeded, preempted, aborted],
            default_outcome=succeeded,
            child_termination_cb=child_term_cb,
            outcome_cb=out_cb,
            input_keys=["in_learn_person"])
        rospy.set_param("/params_learn_and_follow_operator_test/distance_to_human", distToHuman)

        with self:

            setDebugLevel(0)

            Concurrence.add('FOLLOW_OPERATOR',
                            FollowOperator(distToHuman),
                            remapping={"in_learn_person": "in_learn_person"})

            Concurrence.add('LISTEN_COMMANDS',
                            ListenCommands(state_machine_name))
Пример #19
0
def build_ball_timeout_state(grabber_frame, distance_limit, timeout):
    sm = Concurrence(outcomes=["succeeded", "timed_out"],
                     default_outcome="timed_out",
                     output_keys=["ball_location"],
                     outcome_map={
                         "succeeded": {
                             "CLOSEST_BALL": "succeeded"
                         },
                         "timed_out": {
                             "TIMEOUT": "succeeded"
                         }
                     },
                     child_termination_cb=lambda x: True)

    with sm:
        Concurrence.add("CLOSEST_BALL",
                        build_ball_retry_state(grabber_frame, distance_limit),
                        remapping={"position": "ball_location"})
        Concurrence.add("TIMEOUT", Delay(timeout))
    return sm
Пример #20
0
def main():
    rospy.init_node('smach_example_state_machine')

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

    # Open the countainer
    with sm_top:
        StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'})

        # Create the sub SMACH state machine
        sm_con = Concurrence(
            outcomes=['outcome4', 'outcome5'],
            default_outcome='outcome4',
            outcome_map={'outcome5': {
                'FOO': 'outcome2',
                'BAR': 'outcome1'
            }})

        # Open the countainer
        with sm_con:

            # Add states to the container
            Concurrence.add('FOO', Foo())
            Concurrence.add('BAR', Bar())

        StateMachine.add('CON',
                         sm_con,
                         transitions={
                             'outcome4': 'CON',
                             'outcome5': 'outcome6'
                         })

    sis = IntrospectionServer('example', sm_top, '/SM_PATH')
    sis.start()

    # Execute SMACH plan
    outcome = sm_top.execute()

    rospy.spin()
    sis.stop()
Пример #21
0
    def test_concurrence(self):
        """Test concurrent container."""
        sm = StateMachine(['done', 'succeeded'])
        with sm:
            cc = Concurrence(['succeeded', 'done'],
                             default_outcome='done',
                             outcome_map={'succeeded': {
                                 'SETTER': 'done'
                             }})
            sm.add('CONCURRENT', cc)
            with cc:
                Concurrence.add('SETTER', Setter())
                Concurrence.add('GETTER', Getter())

        outcome = sm.execute()

        assert outcome == 'succeeded'
        assert 'a' in cc.userdata
        assert 'b' in cc.userdata
        assert cc.userdata.a == 'A'
        assert cc.userdata.b == 'A'
    def __init__(self, msg_pool, arg_key='tell_argument'):
        input_keys = ['in_location_pose_in_map', 'in_target_object']
        if arg_key:
            input_keys.append(arg_key)
        Concurrence.__init__(self, outcomes=[succeeded, aborted, 'move_failed', 'no_object_found'],
                             default_outcome=aborted,
                             input_keys=input_keys,
                             output_keys=['out_objects_data'],
                             outcome_map={succeeded: {'TELL_GOING_TO_SEARCH': succeeded,
                                                      'MOVE_AND_RECOGNIZE': succeeded},
                                          'no_object_found': {'MOVE_AND_RECOGNIZE': 'no_object_found'},
                                          'move_failed': {'MOVE_AND_RECOGNIZE': 'move_failed'}})

        if type(msg_pool) is str:
            msg_pool = [msg_pool]

        if arg_key is None:
            arg_key = 'tell_argument'
            self.userdata.tell_argument = None  # No argument set!

        # Concurrence container to speak and search simultanously
        with self:
            Concurrence.add('TELL_GOING_TO_SEARCH', SpeakActionFromPoolStateMachine(msg_pool, arg_key=arg_key),
                            remapping={arg_key: arg_key})
            Concurrence.add('MOVE_AND_RECOGNIZE', GoAndRecognizeSM(),
                            remapping={'in_target_object': 'in_target_object',
                                       'in_location_pose_in_map': 'in_location_pose_in_map',
                                       'out_objects_data': 'out_objects_data'})
Пример #23
0
 def __init__(self):
     rospy.init_node('sm_Follow')
     self.sm_FollowMe = Concurrence(outcomes=['succeeded','aborted','error'],
                                     default_outcome ='succeeded',
                                     outcome_map={'succeeded':{'STOP':'succeeded'}},
                                     child_termination_cb =self.child_cb)
     # if one state complete, the Concurrence will give a preempted signal, and will stop the current state in the preempt outcomes
     with self.sm_FollowMe:
         self.meta_Follow = StateMachine(outcomes =['succeeded','aborted','error'])
         with self.meta_Follow:
             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)
     self.sm_FollowMe.execute()
 def __init__(self,
              behavior_pool=None,
              textpool=None,
              wait_before_speak=None):
     input_keys = []
     if not textpool:
         input_keys = ['text']
     use_bpool = True
     if not isinstance(behavior_pool, list):
         use_bpool = False
     Concurrence.__init__(self,
                          outcomes=['succeeded', 'aborted', 'preempted'],
                          input_keys=input_keys,
                          default_outcome='aborted',
                          outcome_map={
                              'succeeded': {
                                  'GESTURE_SPEECH': 'succeeded',
                                  'GESTURE_MOVE': 'succeeded'
                              }
                          })
     with self:
         Concurrence.add(
             'GESTURE_MOVE',
             ExecuteBehavior(behavior_pool)
             if not use_bpool else ExecuteBehaviorFromPoolSM(behavior_pool))
         Concurrence.add(
             'GESTURE_SPEECH',
             SpeechState(wait_before_speak=wait_before_speak,
                         text=textpool,
                         blocking=True) if not textpool else
             SpeechFromPoolSM(wait_before_speak=wait_before_speak,
                              pool=textpool,
                              blocking=True))
Пример #25
0
class sm_Follow():
    def __init__(self):
        rospy.init_node('sm_Follow')

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

        out = self.trace.execute()
        print out

    def trace_child_cb(self, outcome_map):
        if outcome_map['STOP'] == 'stop':
            rospy.logwarn('get the stop signal, stop tracing ........')
            subprocess.call('xterm -e rosnode kill people_tracking &',
                            shell=True)
            return True
        elif outcome_map['STOP'] == 'aborted':
            rospy.logerr('the stop state meet error!')
            return True

        if outcome_map['FOLLOW']:
            rospy.logerr('the follow state meet error!')
            return True
        return False
        #     rospy.logerr('byebye')
        out = self.xm_follow.execute()
        if out == 'succeeded':
            rospy.logwarn('test succeeded')
Пример #26
0
def allign_with_target(target):
    """
    Returns a state that alligns with the specified target. 

    target: string on the form 'GATE' or 'BOUY'

    The returned state is responsible for chaning the circeling direction when needed.

    """

    # TODO: get target position (x,y) from landmark server

    # TODO: create a move_goal
    move_goal = None

    allignment_attempt = Concurrence(
        outcomes=['succeeded', 'preempted', 'wrong_direction'],
        outcome_map={
            'succeeded': {
                'ALLIGNMENT_CHECKER': 'alligned'
            },
            'wrong_direction': {
                'ALLIGNMENT_CHECKER': 'wrong_direction'
            }
        },
        default_outcome=['preempted'],
        child_termination_cb=None  # TODO: should allways terminate
    )

    with allignment_attempt:

        Concurrence.add(
            'CIRCLE_GATE',
            SimpleActionState('controller/move', MoveAction,
                              goal=move_goal)  # TODO
        )
        Concurrence.add('ALLIGNMENT_CHECKER', CBState(allignment_checker))
Пример #27
0
    def __init__(self):
        rospy.init_node('final_project_kl', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        self.keyPointManager = KeyPointManager()
        
         # Create the nav_patrol state machine using a Concurrence container
        self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'],
                                        default_outcome='succeeded',
                                        outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}},
                                        child_termination_cb=self.concurrence_child_termination_cb,
                                        outcome_cb=self.concurrence_outcome_cb)
        # Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine             
        with self.nav_patrol:
           Concurrence.add('SM_NAV', Patrol().getSM())
           Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
        
        
        # Create the top level state machine
        self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])   
        self.sm_top.userdata.sm_ar_tag = None

        with self.sm_top:
            StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'}) 
            StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) 
            
            
        intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = self.sm_top.execute()
        
        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
                
        intro_server.stop()
Пример #28
0
    def test_outcome_cb(self):
        """Test concurrent container that doesnt preempt siblings."""
        cc = Concurrence(['succeeded', 'done'],
                         default_outcome='done',
                         child_termination_cb=lambda so: False,
                         outcome_cb=lambda so: list(set(so.values()))[0])
        with cc:
            Concurrence.add('SETTER', Setter())
            Concurrence.add('GETTER', Getter())

        outcome = cc.execute()

        assert outcome == 'done'
        assert 'a' in cc.userdata
        assert 'b' in cc.userdata
        assert cc.userdata.a == 'A'
        assert cc.userdata.b == 'A'
Пример #29
0
    def __init__(self):
        Concurrence.__init__(
            self,
            outcomes=['exit', 'restart', 'aborted', 'preempted'],
            default_outcome='restart',
            output_keys=['control_infos'],
            child_termination_cb=self.child_termination_cb,
            outcome_cb=self.outcome_cb)

        self.userdata.control_infos = {}

        with self:
            Concurrence.add(
                'CHANGE_CONTROL_MONITOR',
                MonitorState("/change_control",
                             String,
                             self.change_control_cb,
                             input_keys=['control_infos'],
                             output_keys=['control_infos']))
            Concurrence.add('TIMER', Timer())
            Concurrence.add('CONTROL', Control())
Пример #30
0
    def __init__(self):
        Concurrence.__init__(
            self,
            outcomes=['succeeded', 'aborted', 'preempted'],
            default_outcome='succeeded',
            input_keys=['log_mission'],
            output_keys=['log_mission'],
            child_termination_cb=self.child_termination_cb,
            outcome_map={'succeeded': {
                'WAITING': 'finished'
            }})

        self.register_start_cb(self.start_pause)
        self.register_termination_cb(self.termination_pause)

        _min_time_to_go_recharge = 15.0

        @cb_interface(input_keys=['log_mission'],
                      outcomes=['insufficient_time', 'recharge', 'preempted'])
        def check_waiting_time(ud):
            navigation_duration = datetime.datetime.now(
            ) - ud.log_mission['start_time']
            if datetime.timedelta(seconds=ud.log_mission['patrol']['min_duration']) - navigation_duration \
                    > datetime.timedelta(seconds=_min_time_to_go_recharge):
                return 'recharge'
            else:
                return 'succeeded'

        self.recharge = StateMachine(
            input_keys=['log_mission'],
            output_keys=['log_mission'],
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.recharge:
            StateMachine.add('CHECK_WAITING_TIME', CBState(check_waiting_time),
                             {
                                 'recharge': 'RECHARGE',
                                 'insufficient_time': 'succeeded'
                             })

            StateMachine.add('RECHARGE', Recharge())

        with self:
            Concurrence.add('WAITING', Waiting())
            Concurrence.add('CONDITIONAL_RECHARGE', self.recharge)
Пример #31
0
    def test_preempt(self):
        """Test concurrent container that preempts siblings."""
        cc = Concurrence(['succeeded', 'done'],
                         default_outcome='done',
                         child_termination_cb=lambda so: True,
                         outcome_map={
                             'succeeded': {
                                 'SETTER': 'done',
                                 'GETTER': 'preempted'
                             }
                         })
        with cc:
            Concurrence.add('SETTER', Setter())
            Concurrence.add('GETTER', Getter())

        outcome = cc.execute()

        assert outcome == 'succeeded'
        assert 'a' in cc.userdata
        assert 'b' in cc.userdata
        assert cc.userdata.a == 'A'
        assert cc.userdata.b == 'A'
Пример #32
0
 def __init__(self):
     rospy.init_node('smach_haha')
     rospy.on_shutdown(self.shutdown)
     self.smach_top = Concurrence(
         outcomes=['succeeded', 'aborted', 'preempted'],
         default_outcome='succeeded',
         outcome_map={'succeeded': {
             'STATE_1': 'succeeded'
         }},
         child_termination_cb=self.child_termination_cb)
     with self.smach_top:
         self.sm_state2 = StateMachine(outcomes=['succeeded'])
         with self.sm_state2:
             StateMachine.add('FIRST',
                              State2(),
                              transitions={'succeeded': 'SECOND'})
             StateMachine.add('SECOND',
                              State1(),
                              transitions={'succeeded': 'FIRST'})
         Concurrence.add('STATE_1', State1())
         Concurrence.add('STATE_2', self.sm_state2)
     haha = self.smach_top.execute()
Пример #33
0
    def __init__(self):
        rospy.init_node("navgation_test")
        rospy.on_shutdown(self.shutdown)
        self.waypoints=[];
        Location= (Point(3.099,-1.796,0),
                            Point(6.732,-3.260,0),
                            Point(7.058,-0.119,0),
                            Point(-0.595,0.069,0),)
        quaternions=[];
        euler_angles=[0.039,0.056,-3.093,-3.133];

        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)
        for i in range(4):
            self.waypoints.append(Pose(Location[i], quaternions[i]))
            
        point_locations = (('Point1', self.waypoints[0]),
                      ('Point2', self.waypoints[1]),
                      ('Point3', self.waypoints[2]),
                      ('Point4', self.waypoints[3]))
        
        self.room_locations = OrderedDict(point_locations)
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    
        rospy.loginfo("Waiting for move_base action server...")
    
        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))    
        
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
    
        rospy.loginfo("Connected to move_base action server")
    
        nav_states={}
        self.people_in_sight=0
        # Create a navigation task for each room
        for room in self.room_locations.iterkeys():
            nav_goal = MoveBaseGoal()
            nav_goal.target_pose.header.frame_id = 'map'
            nav_goal.target_pose.header.stamp = rospy.Time.now()
            nav_goal.target_pose.pose = self.room_locations[room]
            move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, 
                                                exec_timeout=rospy.Duration(100.0),
                                                server_wait_timeout=rospy.Duration(100.0))
            nav_states[room] = move_base_state
        
        sm_nav1= StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        with sm_nav1:
            StateMachine.add('START',MonitorState("task_comming", xm_Task, self.start_cb),
                                                  transitions={'invalid':'WAIT4DOOR_OPEN',
                                                               'valid':'START',
                                                               'preempted':'WAIT4DOOR_OPEN'})
            
            StateMachine.add('WAIT4DOOR_OPEN',MonitorState("doormsg",door_msg,self.door_is_open_cb),
                                                  transitions={'invalid':'INIT',
                                                               'valid':'WAIT4DOOR_OPEN',
                                                               'preempted':'INIT'})
            StateMachine.add("INIT", ServiceState("pan_srv", xm_Pan,True, response_cb=self.init_cb),transitions={"succeeded":"NAV_1","aborted":"NAV_1"})
            StateMachine.add("NAV_1",nav_states['Point1'],transitions={'succeeded':'','aborted':'','preempted':''})
        sm_nav2=StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_nav2:
            StateMachine.add("NAV_2",nav_states['Point2'],transitions={'succeeded':'','aborted':'Check_People','preempted':''})
            StateMachine.add("Check_People",MonitorState("tracker/tracks_smoothed",TrackArray,self.check_people_cb),
                                                  transitions={'valid':'WAIT',
                                                               'invalid':'WAIT',
                                                               'preempted':'WAIT'})
            StateMachine.add('WAIT',wait(time=5,people_pass=self.people_in_sight),
                                                  transitions={'succeeded':'NAV_2_END'})
            StateMachine.add("NAV_2_END",nav_states['Point2'],transitions={'succeeded':'','aborted':'','preempted':''})
        sm_nav3=StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_nav3:
#             StateMachine.add("NAV_3",nav_states['Point3'],transitions={'succeeded':'WAIT_4_CMD','aborted':'NAV_3','preempted':'NAV_3'})
            sm_follow=Concurrence(outcomes=['succeeded', 'aborted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.concurrence_child_termination_cb,
                                        outcome_cb=self.concurrence_outcome_cb)
            with sm_follow:
                Concurrence.add('START_FOLLOW',MonitorState("task_comming", xm_Task, self.follow_stop_cb))
                co_follow=Concurrence(outcomes=['succeeded','aborted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.co_follow_cb,
                                        outcome_cb=self.co_follow_outcome_cb)
                with co_follow:
                    Concurrence.add('mo_L',MonitorState('people_tf_position',xm_Tracker,
                                              self.pos_M_cb))
                    Concurrence.add('nav', Nav2Waypoint())
                Concurrence.add('FOLLOW',co_follow)
            StateMachine.add('WAIT_4_CMD',MonitorState("task_comming", xm_Task, self.follow_start_cb),
                                                  transitions={'invalid':'FOLLOW_MODE',
                                                               'valid':'FOLLOW_MODE',
                                                               'preempted':'FOLLOW_MODE'})
            StateMachine.add("FOLLOW_MODE",sm_follow,transitions={'succeeded':'','aborted':''})    
            
        sm_nav4=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        with sm_nav4:
            StateMachine.add("WAIT_4_BACK",MonitorState("task_comming",xm_Task,self.cmd_back_cb),transitions={"invalid":"NAV_4","valid":"WAIT_4_BACK","preempted":""})
            StateMachine.add("NAV_4",nav_states['Point4'],transitions={'succeeded':'','aborted':'','preempted':''})
        
        sm_nav_test=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        
        with sm_nav_test:
#             StateMachine.add("NAV_1",sm_nav1,transitions={'succeeded':'NAV_2','aborted':'NAV_2','preempted':'NAV_2'})
#             StateMachine.add("NAV_2",sm_nav2,transitions={'succeeded':'NAV_3','aborted':'NAV_3','preempted':'NAV_3'})
            StateMachine.add("NAV_3",sm_nav3,transitions={'succeeded':'NAV_4','aborted':'NAV_4','preempted':'NAV_4'})
            StateMachine.add("NAV_4",sm_nav4,transitions={'succeeded':'','aborted':'','preempted':''})
        
        sm_outcome = sm_nav_test.execute()
Пример #34
0
    def __init__(self):
        rospy.init_node('HOME_automation_smach', 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]))



	# State machine for light entry 
        self.sm_light_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_light_entry.userdata.day_mode = 1;

        with self.sm_light_entry:
            StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb),
					  transitions={'valid':'LOOK_ENTRY',
					  'invalid':'LIGHT_UP'})
            StateMachine.add('LIGHT_UP', LightEntry(),
                             transitions={'succeeded':'succeeded'})

        # State machine for dark entry 
        self.sm_dark_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_dark_entry.userdata.day_mode = 1;

        with self.sm_dark_entry:
            StateMachine.add('LOOK_ENTRY_OFF', MonitorState("/HOME/entry_noOne", Empty, self.empty_cb),
                                          transitions={'valid':'LOOK_ENTRY_OFF',
                                          'invalid':'LIGHT_DOWN'})
            StateMachine.add('LIGHT_DOWN', DarkEntry(),
                             transitions={'succeeded':'succeeded'})


	# State machine for day mode
        self.sm_day_mode = Concurrence(outcomes=['succeeded','aborted','preempted','go_shower','go_sleep','go_eat','go_out'],
					default_outcome='succeeded',
                                        child_termination_cb=self.daymode_child_termination_cb,
                                        outcome_cb=self.daymode_outcome_cb)
	self.sm_day_mode.userdata.day_mode = 1;

        with self.sm_day_mode:
	    Concurrence.add('LOOK_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb))
	    Concurrence.add('LOOK_LEAVING', MonitorState("/HOME/leaving_home", Empty, self.empty_cb))
	    Concurrence.add('LOOK_SLEEP', MonitorState("/HOME/go_sleep", Empty, self.empty_cb))
	    Concurrence.add('LOOK_EAT', MonitorState("/HOME/go_eat", Empty, self.empty_cb))
	    Concurrence.add('LOOK_ENTRY', self.sm_light_entry)
	    Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)


        # State machine for leaving home
        self.sm_leaving_home = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_leaving_home:
            StateMachine.add('LEAV', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})

        # State machine for going to sleep
        self.sm_going_sleep = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_going_sleep:
            StateMachine.add('GOING_SLEEP', GoingSleep(),
                             transitions={'succeeded':'succeeded'})


	# State machine for day mode
        self.sm_wait_bed = Concurrence(outcomes=['succeeded','aborted','preempted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.useless_child_termination_cb,
                                        outcome_cb=self.useless_outcome_cb)

        with self.sm_wait_bed:
            Concurrence.add('LOOK_INBED', MonitorState("/METAWATCH/button2", Empty, self.empty_cb))
            Concurrence.add('TIMEOUT', TimeoutToBed())


	# State machine for in bed
        self.sm_in_bed = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_in_bed:
            StateMachine.add('IN_BED', InBed(),
                             transitions={'succeeded':'succeeded'})



	# State machine for light entry 
        self.sm_lightn_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_lightn_entry.userdata.day_mode = 0;

        with self.sm_lightn_entry:
            StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb),
                                          transitions={'valid':'LOOK_ENTRY',
                                          'invalid':'LIGHT_UP'})
            StateMachine.add('LIGHT_UP', LightEntry(),
                             transitions={'succeeded':'succeeded'})


	# State machine for night mode
        self.sm_night_mode = Concurrence(outcomes=['succeeded','aborted','preempted','wake_up'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.nightmode_child_termination_cb,
                                        outcome_cb=self.nightmode_outcome_cb)
        self.sm_night_mode.userdata.day_mode = 0;

        with self.sm_night_mode:
            Concurrence.add('LOOK_WAKE', MonitorState("/HOME/wake_up", Empty, self.empty_cb))
            Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry)
            Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)


        # State machine for night mode
        #self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted'])
	#self.sm_night_mode.userdata.day_mode = 0;

        #with self.sm_night_mode:
        #    StateMachine.add('NIGHT_MOD', Pause(),
        #                     transitions={'succeeded':'succeeded',
        #                                  'aborted':'aborted'})

        # State machine for waking up 
        self.sm_waking_up = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_waking_up.userdata.day_mode = 0;

        with self.sm_waking_up:
            StateMachine.add('WAKING_UP', WakingUp(),
                             transitions={'succeeded':'succeeded'})

	# State machine for waking up 
        self.sm_going_eat = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_going_eat.userdata.day_mode = 1;

        with self.sm_going_eat:
            StateMachine.add('EATTTTTTTT', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})





	# State machine for home
        self.sm_home = StateMachine(outcomes=['succeeded','aborted','preempted','going_out'])

        with self.sm_home:
            StateMachine.add('DAY_MODE', self.sm_day_mode,
                             transitions={'succeeded':'DAY_MODE',
					  'go_shower':'PREPARING_SHOWER',
                                          'go_sleep':'GOING_SLEEP',
                                          'go_eat':'GOING_EAT',
                                          'go_out':'LEAVING_HOME',
                                          'aborted':'aborted'})
            StateMachine.add('PREPARING_SHOWER', PreparingShower(),
                             transitions={'succeeded':'GO_SHOWER',
                                          'aborted':'aborted'})
            StateMachine.add('GO_SHOWER', GoShower(),
                             transitions={'succeeded':'STOP_SHOWER',
                                          'aborted':'aborted'})
            StateMachine.add('STOP_SHOWER', StopShower(),
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('LEAVING_HOME', self.sm_leaving_home,
                             transitions={'succeeded':'going_out',
                                          'aborted':'aborted'})
            StateMachine.add('GOING_SLEEP', self.sm_going_sleep,
                             transitions={'succeeded':'WAIT_TO_BED',
                                          'aborted':'aborted'})
	    StateMachine.add('WAIT_TO_BED', self.sm_wait_bed,
                             transitions={'succeeded':'IN_BED',
					  'preempted':'IN_BED',
                                          'aborted':'aborted'})
            StateMachine.add('IN_BED', self.sm_in_bed,
                             transitions={'succeeded':'NIGHT_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('NIGHT_MODE', self.sm_night_mode,
                             transitions={'succeeded':'NIGHT_MODE',
					  'wake_up':'WAKING_UP',
                                          'aborted':'aborted'})
            StateMachine.add('WAKING_UP', self.sm_waking_up,
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('GOING_EAT', self.sm_going_eat,
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})






	# State machine for waking up 
        self.sm_guarding = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_guarding:
            StateMachine.add('GUARD', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})



	# State machine with concurrence
        self.sm_incoming_home = Concurrence(outcomes=['succeeded', 'aborted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.incoming_child_termination_cb,
                                        outcome_cb=self.incoming_outcome_cb)

        # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine             
        with self.sm_incoming_home:
           Concurrence.add('LOOK_CONNECTION', MonitorState("/METAWATCH/connected", Empty, self.empty_cb))
           Concurrence.add('LOOK_ENTERING', MonitorState("/HOME/entry_door_open", Empty, self.empty_cb))





        # State machine for away
        self.sm_away = StateMachine(outcomes=['succeeded','aborted','preempted','entering_home'])

        with self.sm_away:
            StateMachine.add('GUARDING_MODE', self.sm_guarding,
                             transitions={'succeeded':'INCOMING_HOME',
                                          'aborted':'aborted'})
            StateMachine.add('INCOMING_HOME', self.sm_incoming_home,
                             transitions={'succeeded':'entering_home',
                                          'aborted':'aborted'})




        # 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('AT_HOME', self.sm_home, transitions={'succeeded':'AT_HOME', 'going_out':'AWAY'})
            #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
            StateMachine.add('AWAY', self.sm_away, transitions={'succeeded':'AWAY', 'entering_home':'AT_HOME'})








        # 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()
Пример #35
0
    def __init__(self):
        rospy.init_node("deliver_food", anonymous=False)
        self.initialize_destination()
        self.lalala = 100

        # Track success rate of getting to the goal locations
        self.n_succeeded = 0
        self.n_aborted = 0
        self.n_preempted = 0

        nav_states = {}

        for room in self.room_locations.iterkeys():
            nav_goal = MoveBaseGoal()
            nav_goal.target_pose.header.frame_id = "map"
            nav_goal.target_pose.pose = self.room_locations[room]
            move_base_state = SimpleActionState(
                "move_base",
                MoveBaseAction,
                goal=nav_goal,
                result_cb=self.move_base_result_cb,
                exec_timeout=rospy.Duration(60.0),
                server_wait_timeout=rospy.Duration(10.0),
            )
            nav_states[room] = move_base_state
            rospy.loginfo(
                room
                + " -> ["
                + str(round(self.room_locations[room].position.x, 2))
                + ", "
                + str(round(self.room_locations[room].position.y, 2))
                + "]"
            )

        sm_rotate_search = Concurrence(
            outcomes=["find", "not_find"],
            default_outcome="not_find",
            child_termination_cb=self.concurrence_child_termination_callback,
            outcome_cb=self.concurrence_outcome_callback,
        )

        with sm_rotate_search:
            Concurrence.add("ROTATE", Rotate360(0.4, 2 * pi))
            Concurrence.add("SEARCH", SearchTable())

        sm_table1 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table1:
            StateMachine.add(
                "GOTO_TABLE1",
                nav_states["table1"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
            )

        sm_table2 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table2:
            StateMachine.add(
                "GOTO_TABLE2",
                nav_states["table2"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
            )

        sm_table3 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table3:
            StateMachine.add(
                "GOTO_TABLE3",
                nav_states["table3"],
                transitions={
                    "succeeded": "ROTATE_SEARCH",
                    "aborted": "GOTO_CHECKPOINT_2",
                    "preempted": "GOTO_CHECKPOINT_2",
                },
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a"}
            )

            # if something wrong when we tried to go to the table

            StateMachine.add(
                "GOTO_CHECKPOINT_2",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_CHECKPOINT_1"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_1",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_CHECKPOINT_2z", "aborted": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_2z",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE3_2",
                nav_states["table3"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
            )

            StateMachine.add(
                "CLEARING_NOISE_GO",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_TABLE3_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE3_3",
                nav_states["table3"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
            )

            # if something wrong when we tried to go back to kitchen

            StateMachine.add(
                "GOTO_CHECKPOINT_1a",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_2a",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_a",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_2",
                nav_states["kitchen"],
                transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_b",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
            )

        sm_table4 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table4:
            StateMachine.add(
                "GOTO_TABLE4",
                nav_states["table4"],
                transitions={
                    "succeeded": "ROTATE_SEARCH",
                    "aborted": "GOTO_CHECKPOINT_2",
                    "preempted": "GOTO_CHECKPOINT_2",
                },
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN",
                nav_states["kitchen"],
                transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
            )

            # if something wrong when we tried to go to the table

            StateMachine.add(
                "GOTO_CHECKPOINT_2",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_TABLE4_2", "aborted": "GOTO_CHECKPOINT_1"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_1",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE4_2",
                nav_states["table4"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
            )

            StateMachine.add(
                "CLEARING_NOISE_GO",
                Rotate360(0.4, 2 * pi),
                transitions={"full_rotate": "GOTO_TABLE4_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE4_3",
                nav_states["table4"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
            )

            # if something wrong when we tried to go back to kitchen

            StateMachine.add(
                "GOTO_CHECKPOINT_1a",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_2a",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_a",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_2",
                nav_states["kitchen"],
                transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_b",
                Rotate360(0.4, 2 * pi),
                transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
            )

        sm_table5 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table5:
            StateMachine.add(
                "GOTO_TABLE5",
                nav_states["table5"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
            )

        sm_table6 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
        with sm_table6:
            StateMachine.add(
                "GOTO_TABLE6",
                nav_states["table6"],
                transitions={
                    "succeeded": "ROTATE_SEARCH",
                    "aborted": "GOTO_CHECKPOINT_2",
                    "preempted": "GOTO_CHECKPOINT_2",
                },
            )
            StateMachine.add(
                "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
            )
            StateMachine.add(
                "DO_STUFFS",
                DoStuffs(5),
                transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
            )
            StateMachine.add(
                "GOTO_KITCHEN",
                nav_states["kitchen"],
                transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
            )

            # if something wrong when we tried to go to the table

            StateMachine.add(
                "GOTO_CHECKPOINT_2",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_TABLE6_2", "aborted": "GOTO_CHECKPOINT_1"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_1",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE6_2",
                nav_states["table6"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
            )

            StateMachine.add(
                "CLEARING_NOISE_GO",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_TABLE6_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_TABLE6_3",
                nav_states["table6"],
                transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
            )

            # if something wrong when we tried to go back to kitchen

            StateMachine.add(
                "GOTO_CHECKPOINT_1a",
                nav_states["checkpoint1"],
                transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_CHECKPOINT_2a",
                nav_states["checkpoint2"],
                transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_a",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_2",
                nav_states["kitchen"],
                transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
            )

            StateMachine.add(
                "CLEARING_NOISE_BACK_b",
                Rotate360(0.8, 2 * pi),
                transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
            )

            StateMachine.add(
                "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
            )

        # let's initialize the overall state machine
        sm_deliverfood = StateMachine(outcomes=["succeeded", "aborted", "preempted"])

        with sm_deliverfood:
            StateMachine.add("STARTING_TASK", Welcome(), transitions={"succeeded": "COMPUTER_VISION_TASK"})
            StateMachine.add(
                "COMPUTER_VISION_TASK",
                ComputerVision(),
                transitions={
                    "detect1": "TABLE_ONE_TASK",
                    "detect2": "TABLE_TWO_TASK",
                    "detect3": "TABLE_THREE_TASK",
                    "detect4": "TABLE_FOUR_TASK",
                    "detect5": "TABLE_FIVE_TASK",
                    "detect6": "TABLE_SIX_TASK",
                    "preempted": "",
                },
            )
            StateMachine.add(
                "TABLE_ONE_TASK",
                sm_table1,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )
            StateMachine.add(
                "TABLE_TWO_TASK",
                sm_table2,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )
            StateMachine.add(
                "TABLE_THREE_TASK",
                sm_table3,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )
            StateMachine.add(
                "TABLE_FOUR_TASK",
                sm_table4,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )
            StateMachine.add(
                "TABLE_FIVE_TASK",
                sm_table5,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )
            StateMachine.add(
                "TABLE_SIX_TASK",
                sm_table6,
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )

            StateMachine.add(
                "GOTO_KITCHEN",
                nav_states["kitchen"],
                transitions={
                    "succeeded": "COMPUTER_VISION_TASK",
                    "aborted": "GOTO_KITCHEN",
                    "preempted": "GOTO_KITCHEN",
                },
            )

        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer("deliver_food", sm_deliverfood, "/SM_ROOT")
        intro_server.start()

        # Execute the state machine
        sm_outcome = sm_deliverfood.execute()
        rospy.on_shutdown(self.shutdown)
    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

        self.robot_side = 1






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

        with self.sm_action1:            
            StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
                             transitions={'succeeded':'NAV_WAYPOINT',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
                                          'aborted':'aborted'})
#            StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
#                             transitions={'succeeded':'CLOSE_MANDIBLES',
#                                          'aborted':'aborted'})
	    StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
                             output_keys=['waypoint_out'],
			     input_keys=['robot_side']),
                             transitions={'succeeded':'HCLOSE_MANDIBLES',
                                          'aborted':'HCLOSE_MANDIBLES'},
                             remapping={'waypoint_out':'patrol_waypoint'})
            StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
                             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.3;
	self.sm_action2.userdata.mandibles_sleep = 0.1

        with self.sm_action2:
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'OPEN_MANDIBLES',
                                          'aborted':'aborted'})
            StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
                             transitions={'succeeded':'MOVE_BACKWARD',
                                          'aborted':'aborted'})
	    StateMachine.add('MOVE_BACKWARD', MoveForward(),
                             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'])
	self.sm_action3.userdata.mandibles_sleep = 0.1
	self.sm_action3.userdata.speed = -0.1;
        self.sm_action3.userdata.distance = 0.2;

        with self.sm_action3:
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'MOVE_PUSH',
                                          'aborted':'aborted'})
	    StateMachine.add('MOVE_PUSH', MovePush(),
                             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'])
	self.sm_action4.userdata.mandibles_sleep = 0.1
	self.sm_action4.userdata.speed_x = 0.1
	self.sm_action4.userdata.speed_y = 0.1
	self.sm_action4.userdata.distance = 0.5;

        with self.sm_action4:
	    StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
                             transitions={'succeeded':'NAV_WAYPOINT',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'SLIDE',
                                          'aborted':'aborted'})
	    StateMachine.add('SLIDE', Slide(),
                             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'])
	self.sm_action5.userdata.mandibles_sleep = 0.1

        with self.sm_action5:
	    StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
                             transitions={'succeeded':'NAV_WAYPOINT',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
                                          'aborted':'aborted'})
#            StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
#                             transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
#                                          'aborted':'aborted'})
	    StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
                             output_keys=['waypoint_out'],
			     input_keys=['robot_side']),
                             transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
                                          'aborted':'ALMOSTCLOSE_MANDIBLES'})
	    StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
                             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'])
	self.sm_action6.userdata.mandibles_sleep = 0.1
	self.sm_action6.userdata.speed = 0.1;
        self.sm_action6.userdata.distance = 0.2;

        with self.sm_action6:
            StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
                             transitions={'succeeded':'NAV_WAYPOINT',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'FORWARD',
                                          'aborted':'aborted'})
	    StateMachine.add('FORWARD', MoveForward(),
                             transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
                                          'aborted':'aborted'})
#	    StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
#                             transitions={'succeeded':'HCLOSE_MANDIBLES',
#                                          'aborted':'aborted'})
	    StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
                             output_keys=['waypoint_out'],
			     input_keys=['robot_side']),
                             transitions={'succeeded':'HCLOSE_MANDIBLES',
                                          'aborted':'HCLOSE_MANDIBLES'})
	    StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})

	# State machine for Action7
        self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
	self.sm_action7.userdata.mandibles_sleep = 0.1

        with self.sm_action7:
            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','action7','aborted','succeeded','preempted']),
                             transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','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('SM_ACTION7', self.sm_action7, 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:

#	    @smach.cb_interface()
#    	    def requestPrioCube_cb(userdata, request):
#       		update_request = UpdatePriority().Request
#       		update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
#       		update_request.goal.pose.position.y = 0.800
#			update_request.prio = 100
#       		return update_request


            StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
            StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
	    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()
Пример #37
0
    def __init__(self, target_object_key=None, target_frame='/map', tsp_route=TSP_route):
        input_keys = ['in_room_name']
        if target_object_key is not None:
            input_keys.append(target_object_key)

        smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
                                    input_keys=input_keys,
                                    output_keys=['out_object_found', 'out_location_inside_room_name'])

        if target_object_key is None:
            target_object_key = 'in_target_object'
            self.userdata.in_target_object = ''  # No target object set! Before set to str(None), changed for SearchObjectWCSM
            going_to_pool = ["I'm going to the %s to look for objects.", "Maybe I find objects on the %s. I'll have a look.",
                             "Why don't I search objects on the %s?", "I think there can be objects on the %s."]
            object_not_found_pool = ["I can't see any object on the %s.", "It seems that there aren't objects on the %s.",
                                     "I haven't found any object on the %s."]
        else:
            going_to_pool = ["I'm going to the %s to look if it's there.", "Maybe I find it on the %s. I'll have a look.",
                             "Why don't I search it on the %s?", "I think it can be on the %s."]
            object_not_found_pool = ["I can't see it on the %s.", "It seems that it isn't on the %s.",
                                     "I haven't found it on the %s."]

        object_found_pool = ["I can see the %s on the %s.", "Is that the %s on the %s? I think so!",
                             "The %s is just there on the %s.", "I found the %s on the %s.",
                             "Can't you see the %s? Is there, on the %s!"]

        self.userdata.objects_data = None  # To avoid errors the first time the check_if_remaining is called
        self.userdata.sensing_route = None  # Because at the very beginning there's no route
        self.userdata.furniture_route = None  # Because at the very beginning there's no route
        self.userdata.out_object_found = None  # To avoid errors if there's an abort.

        with self:
            StateMachine.add('CHECK_IF_OBJECTS_REMAINING', CheckRemaining(),
                             remapping={'in_obj_list': 'objects_data'},
                             transitions={'empty': 'CHECK_IF_FURNITURE_ROUTE_EMPTY',
                                          'remaining': 'TELL_GO_RECOGNIZE'})

            @smach.cb_interface(input_keys=['in_route', 'in_furniture_poses', 'in_orientations', 'in_furniture_name_list'],
                                output_keys=['out_remaining_poses', 'out_remaining_orientations', 'out_remaining_names'],
                                outcomes=['remaining', 'empty'])
            def check_furniture_route_emptiness(userdata):
                if userdata.in_route:
                    # We get the remaining poses and orientations of the furniture route
                    # and then we'll recalculate the route through that remaining nodes
                    remaining_poses = []
                    remaining_orientations = []
                    remaining_names = []
                    for index in userdata.in_route:  # furniture route has only the indices of the elements
                        remaining_poses.append(userdata.in_furniture_poses[index])
                        remaining_orientations.append(userdata.in_orientations[index])
                        remaining_names.append(userdata.in_furniture_name_list[index])
                    userdata.out_remaining_poses = remaining_poses
                    userdata.out_remaining_orientations = remaining_orientations
                    userdata.out_remaining_names = remaining_names
                    return 'remaining'
                return 'empty'

            StateMachine.add('CHECK_IF_FURNITURE_ROUTE_EMPTY',
                             CBState(check_furniture_route_emptiness,
                                     input_keys=['in_route', 'in_furniture_poses', 'in_orientations', 'in_furniture_name_list'],
                                     output_keys=['out_remaining_poses', 'out_remaining_orientations', 'out_remaining_names'],
                                     outcomes=[succeeded, 'empty']),
                             remapping={'in_route': 'furniture_route',
                                        'in_furniture_poses': 'furniture_detect_poses',
                                        'in_orientations': 'furniture_orientation_data',
                                        'out_remaining_poses': 'furniture_detect_poses',
                                        'out_remaining_orientations': 'furniture_orientation_data',
                                        'in_furniture_name_list': 'furniture_name_list',
                                        'out_remaining_names': 'furniture_name_list'},
                             transitions={'remaining': 'FURNITURE_TSP_ROUTE',
                                          'empty': 'CHECK_IF_SENSING_ROUTE_EMPTY'})

            @smach.cb_interface(input_keys=['in_route'], output_keys=['out_remaining_nodes'],
                                outcomes=['remaining', 'empty'])
            def check_sensing_route_emptiness(userdata):
                if userdata.in_route:
                    # We get the remaining nodes of the route
                    # and then we'll recalculate the route through that remaining nodes
                    userdata.out_remaining_nodes = userdata.in_route  # The route itself has the nodes ordered.
                    return 'remaining'
                return 'empty'

            StateMachine.add('CHECK_IF_SENSING_ROUTE_EMPTY',
                             CBState(check_sensing_route_emptiness,
                                     input_keys=['in_route'],
                                     output_keys=['out_remaining_nodes'],
                                     outcomes=[succeeded, 'empty']),
                             remapping={'in_route': 'sensing_route',
                                        'out_remaining_nodes': 'sens_locations'},
                             transitions={'remaining': 'SENSING_LOCATIONS_TSP_ROUTE',
                                          'empty': 'CALCULATE_FURNITURE_SENSING_LOCATIONS'})

            StateMachine.add('CALCULATE_FURNITURE_SENSING_LOCATIONS', CalculateSensingLocations(),
                             remapping={'in_room_name': 'in_room_name', 'out_guards': 'sens_locations'},
                             transitions={succeeded: 'SENSING_LOCATIONS_TSP_ROUTE', 'no_corner_info': aborted})

            StateMachine.add('SENSING_LOCATIONS_TSP_ROUTE', TSPState(HC=HC),
                             remapping={'in_nodes': 'sens_locations', 'out_route': 'sensing_route'},
                             transitions={succeeded: 'GET_NEXT_SENSING_LOCATION'})

            @smach.cb_interface(input_keys=['in_sensing_route'], output_keys=['out_sensing_route', 'out_next_sensing_pose'],
                                outcomes=[succeeded, 'empty_route'])
            def get_next_loc_from_route(userdata):
                # Returns the next sensing location to go and removes it from the route
                if not userdata.in_sensing_route:  # The sensing route is empty...
                    return 'empty_route'
                next_pos = userdata.in_sensing_route[0]
                userdata.out_sensing_route = userdata.in_sensing_route[1:]
                pose = Pose()
                pose.position.x = next_pos[0]
                pose.position.y = next_pos[1]
                pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.57))  # FIXME is it possible to avoid a fixed orientation?
                userdata.out_next_sensing_pose = pose
                return succeeded

            StateMachine.add('GET_NEXT_SENSING_LOCATION',
                             CBState(get_next_loc_from_route,
                                     input_keys=['in_sensing_route'],
                                     output_keys=['out_sensing_route', 'out_next_sensing_pose'],
                                     outcomes=[succeeded, 'empty_route']),
                             remapping={'out_sensing_route': 'sensing_route',
                                        'in_sensing_route': 'sensing_route',
                                        'out_next_sensing_pose': 'next_sensing_pose'},
                             transitions={succeeded: 'MOVE_TO_SENSING_LOCATION',
                                          'empty_route': 'CALCULATE_FURNITURE_SENSING_LOCATIONS'})

            StateMachine.add('MOVE_TO_SENSING_LOCATION',
                             MoveActionState("/map", goal_key='next_sensing_pose'),
                             transitions={succeeded: 'DETECT_FURNITURE_AND_ANNOUNCE',
                                          aborted: 'GET_NEXT_SENSING_LOCATION'})  # FIXME if aborted we get the next pose.

            tell_and_search = Concurrence(outcomes=[succeeded, aborted, preempted, 'no_furniture'],
                                          default_outcome=aborted,
                                          output_keys=['furniture_detect_poses',
                                                       'furniture_orientation_data', 'furniture_name_list'],
                                          outcome_map={succeeded: {'ANNOUNCE_SEARCHING': succeeded,
                                                                   'DETECT_FURNITURE_ZONE': succeeded},
                                                       'no_furniture': {'DETECT_FURNITURE_ZONE': 'no_furniture'}})
            with tell_and_search:
                searching_furniture_pool = ["I'm looking for places that can contain objects.",
                                            "I'm searching a place that can have objects.",
                                            "I'm searching the best place to find objects."]
                Concurrence.add('ANNOUNCE_SEARCHING',
                                SpeakActionFromPoolStateMachine(searching_furniture_pool))

                Concurrence.add('DETECT_FURNITURE_ZONE',
                                DetectFurnitureOfZoneSM(distance_treshold=DIST_BETWEEN_FURNITURE, dist_to_furniture=DIST_TO_FURNITURE),
                                remapping={'out_furniture_pose_list': 'furniture_detect_poses',
                                           'out_furniture_orientation_list': 'furniture_orientation_data',
                                           'out_furniture_name_list': 'furniture_name_list'})

            transition_route = 'FURNITURE_TSP_ROUTE' if tsp_route else 'FURNITURE_PROBABILITY_ROUTE'
            StateMachine.add('DETECT_FURNITURE_AND_ANNOUNCE', tell_and_search,
                             transitions={succeeded: transition_route, 'no_furniture': 'GET_NEXT_SENSING_LOCATION'},
                             remapping={'furniture_detect_poses': 'furniture_detect_poses',
                                        'furniture_name_list': 'furniture_name_list',
                                        'furniture_orientation_data': 'furniture_orientation_data'})


            #FIXME filter poses and everything by probability or something like that?
            # @smach.cb_interface(input_keys=['in_furniture_detect_data'], output_keys=['out_furniture_node_list'],
            #                     outcomes=[succeeded])
            # def check_furniture_data(userdata):
            #     #FIXME -> May be necessary to filter in some way the already visited furniture...
            #     #furniture_detect_data is a list of tuples (pos, orientation), so we get only the poses for the TSP
            #     userdata.out_furniture_node_list = reduce(lambda acc, x: acc + [x[0]], userdata.in_furniture_detect_data, [])

            #     return succeeded

            # StateMachine.add('CHECK_FURNITURE_DATA',
            #                  CBState(check_furniture_data, input_keys=['in_furniture_detect_data'],
            #                          output_keys=['out_object_detection_pose'],
            #                          outcomes=[succeeded]),
            #                  remapping={'in_furniture_detect_data': 'furniture_detect_data',
            #                             'out_furniture_node_list': 'furniture_node_list'},
            #                  transitions={succeeded: 'TELL_GO_RECOGNIZE'})

            StateMachine.add('FURNITURE_TSP_ROUTE', TSPState(HC=HC, indices=True),
                             remapping={'in_nodes': 'furniture_detect_poses',
                                        'out_route': 'furniture_route'},
                             transitions={succeeded: 'GET_NEXT_FURNITURE_LOCATION'})

            @smach.cb_interface(input_keys=['in_nodes', 'in_furniture_name_list'],
                                output_keys=['out_route', 'out_furniture_name_list'],
                                outcomes=[succeeded])
            def create_probability_route(userdata):
                furn_prob_map = rospy.get_param(FURN_PROB_PARAM)
                n = len(userdata.in_nodes)
                route = range(n)
                # Get a list of tuples (probability, index_of_name)
                prob_index = zip(reduce(lambda acc, x: acc + [furn_prob_map[x]], userdata.in_furniture_name_list, []), route)
                prob_index.sort(key=lambda x: x[0], reverse=True)  # Sort by probability

                robot_pose = Pose()
                robot_pose.position.x = 0
                robot_pose.position.y = 0
                robot_pose.position.z = 0
                # Get robot's pose in /map coordinates
                robot_pose = transform_pose(robot_pose, '/base_link', '/map', timeout=3)
                actual = (robot_pose.position.x, robot_pose.position.y)

                route[0] = prob_index[0][1]
                for i in xrange(1, n):
                    route[i] = prob_index[i][1]
                    if prob_index[i][0] == 0:  # If probability is 0 we don't look at it.
                        del route[i]
                        del userdata.in_furniture_name_list[i]
                    elif prob_index[i][0] == prob_index[i-1][0]:  # If two things have the same probability, get the closer one
                        di_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i]], actual)
                        di1_r = ofb_utils.euclidean_distance(userdata.in_nodes[route[i-1]], actual)
                        if di_r < di1_r:   # The index at i is nearer than the one at i-1
                            route[i] = route[i-1]
                            route[i-1] = prob_index[i][1]
                userdata.out_route = route
                userdata.out_furniture_list = userdata.in_furniture_name_list
                return succeeded

            StateMachine.add('FURNITURE_PROBABILITY_ROUTE',
                             CBState(create_probability_route,
                                     input_keys=['in_nodes', 'in_furniture_name_list'],
                                     output_keys=['out_route', 'out_furniture_name_list'],
                                     outcomes=[succeeded]),
                             remapping={'in_nodes': 'furniture_detect_poses',
                                        'in_furniture_name_list': 'furniture_name_list',
                                        'out_furniture_name_list': 'furniture_name_list'
                                        'out_route': 'furniture_route'},
                             transitions={succeeded: 'GET_NEXT_FURNITURE_LOCATION'})

            @smach.cb_interface(input_keys=['in_furniture_route', 'in_orient_list', 'in_pose_list', 'in_furniture_name_list'],
                                output_keys=['out_furniture_route', 'out_object_detection_pose', 'out_furniture_name'],
                                outcomes=[succeeded, 'empty_route'])
            def get_next_loc_from_furniture_route(userdata):
                # Returns the next furniture location to go and removes it from the route
                if not userdata.in_furniture_route:  # The route is empty, so we recalculate the sensing route and go again
                    return 'empty_route'

                node_index = userdata.in_furniture_route[0]
                print('node_index: %d, length in_furniture_route: %d' % (node_index, len(userdata.in_furniture_route)))
                next_pos = userdata.in_pose_list[node_index]

                # Only the index is removed from the list, to preserve the index order of the other elements
                userdata.out_furniture_route = userdata.in_furniture_route[1:]
                pose = Pose()
                pose.position.x = next_pos[0]
                pose.position.y = next_pos[1]
                pose.orientation = userdata.in_orient_list[node_index]
                userdata.out_object_detection_pose = pose
                userdata.out_furniture_name = userdata.in_furniture_name_list[node_index]
                return succeeded

            StateMachine.add('GET_NEXT_FURNITURE_LOCATION',
                             CBState(get_next_loc_from_furniture_route,
                                     input_keys=['in_furniture_route', 'in_orient_list', 'in_pose_list'],
                                     output_keys=['out_furniture_route', 'out_object_detection_pose', 'out_furniture_name'],
                                     outcomes=[succeeded, 'empty_route']),
                             remapping={'out_furniture_route': 'furniture_route',
                                        'in_furniture_route': 'furniture_route',
                                        'in_orient_list': 'furniture_orientation_data',
                                        'in_pose_list': 'furniture_detect_poses',
                                        'in_furniture_name_list': 'furniture_name_list',
                                        'out_object_detection_pose': 'object_detection_pose',
                                        'out_furniture_name': 'out_location_inside_room_name'},
                             transitions={succeeded: 'TELL_GO_RECOGNIZE',
                                          'empty_route': 'SENSING_LOCATIONS_TSP_ROUTE'})

            StateMachine.add('TELL_GO_RECOGNIZE', TellGoRecognizeSM(msg_pool=going_to_pool, arg_key='location_name'),
                             remapping={'location_name': 'out_location_inside_room_name',
                                        'out_objects_data': 'objects_data',
                                        'in_target_object': target_object_key,
                                        'in_location_pose_in_map': 'object_detection_pose'},
                             transitions={'move_failed': 'GET_NEXT_SENSING_LOCATION',
                                          'no_object_found': 'CHECK_IF_OBJECT_FOUND',
                                          succeeded: 'CHECK_IF_OBJECT_FOUND',
                                          aborted: aborted})  # FIXME aborted should abort everything?

            StateMachine.add('CHECK_IF_OBJECT_FOUND',
                             CheckObjectAndRemoveFromList(target_frame),
                             remapping={'out_object_found': 'out_object_found',
                                        'in_objects_data': 'objects_data',
                                        'out_objects_data': 'objects_data',
                                        'in_target_object': target_object_key},
                             transitions={succeeded: 'PREPARE_POOL_ARGS', aborted: 'TELL_NO_OBJECT_FOUND'})

            StateMachine.add('TELL_NO_OBJECT_FOUND',
                             SpeakActionFromPoolStateMachine(object_not_found_pool, arg_key='location_name'),
                             remapping={'location_name': 'out_location_inside_room_name'},
                             transitions={succeeded: 'GET_NEXT_SENSING_LOCATION', aborted: 'GET_NEXT_SENSING_LOCATION'})

            @smach.cb_interface(input_keys=['in_first_object', 'in_location_name'], output_keys=['out_tell_arg'],
                                outcomes=[succeeded])
            def prepare_userdata(userdata):
                userdata.out_tell_arg = (userdata.in_first_object.name, userdata.in_location_name)
                return succeeded

            StateMachine.add('PREPARE_POOL_ARGS',
                             CBState(prepare_userdata,
                                     input_keys=['in_first_object', 'in_location_name'],
                                     output_keys=['out_tell_arg'], outcomes=[succeeded]),
                             remapping={'in_first_object': 'out_object_found',
                                        'out_tell_arg': 'tell_arg',
                                        'in_location_name': 'out_location_inside_room_name'},
                             transitions={succeeded: 'TELL_OBJECT_RECOGNIZED'})

            StateMachine.add('TELL_OBJECT_RECOGNIZED',
                             SpeakActionFromPoolStateMachine(object_found_pool, arg_key="tell_arg"),
                             remapping={'tell_arg': 'tell_arg'},
                             transitions={succeeded: succeeded, aborted: aborted})
Пример #38
0
    def __init__(self):
        rospy.init_node('whoiswho_nopick')
        rospy.logwarn('start who is who test')
        self.smach_bool =False
        rospy.on_shutdown(self.shutdown)
        # subprocess.call("xterm -e rosrun xm_speech xm_speech_client.py &", shell = True) 
        # add waypoints into the list
        self.waypoints=[]

        location= (Point(1.003, -0.217, 0.000),  #找人数的点
                   Point( 5.284, -1.246, 0.000),   #出门抓东西的点
                   Point( 0.000,0.000,0.000))     #结束出门的点
        quaternions=(Quaternion(0.000, 0.000, 0.723, 0.691),
                     Quaternion(0.000, 0.000,0.050, 0.999),
                     Quaternion(0.000,0.000,0.000, 1))

        # euler_angles=[0.000,3.1415,0.000]
        # for angle in euler_angles:
        #     q_angle = quaternion_from_euler(0, 0, angle, axes="sxyz")
        #     q = Quaternion(*q_angle)
        #     quaternions.append(q)
        for i in range(3):
            self.waypoints.append(Pose(location[i], quaternions[i]))

        # the locate can specified by ourselves
        self.sm_EnterRoom = StateMachine(outcomes = ['succeeded','aborted','error'])
        with self.sm_EnterRoom:
            # rospy.logerr('sm_EnterRoom add State')
            # arm go to the nav_pose in the srdf
            # self.sm_EnterRoom.userdata.arm_mode =0
            # self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped()
            # StateMachine.add('NAV_POSE',
            #                     ArmCmd(),
            #                     transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'},
            #                     remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'})
            # wait the door to open
            # StateMachine.add('DOOR_DETECT',
            #                     DoorDetect().door_detect_,
            #                     transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'})
            # simple delay 5s
            # self.sm_EnterRoom.userdata.rec = 5.0
            # StateMachine.add('WAIT',
            #                     Wait(),
            #                     transitions={'succeeded':'NAV_1','error':'error'},
            #                     remapping ={'rec':'rec'})

            # self.sm_EnterRoom.userdata.point = Point(1.5,0.0,0.0)
            # StateMachine.add('SIMPLE_MOVE',
            #                     SimpleMove_move(),
            #                     transitions={'succeeded':'NAV_1','aborted':'NAV_1','error':'error'},
            #                     remapping={'point':'point'})
            
            # navstack to room of crowds
            # waypoints are the list of Pose fit the data type need
            self.sm_EnterRoom.userdata.start_waypoint  = self.waypoints[0]
            # self.sm_EnterRoom.userdata.nav_mode =0
            StateMachine.add('NAV_1',
                                NavStack(),
                                transitions={'succeeded':'SELF_INTRO','aborted':'NAV_1','error':'error'},
                                remapping = {'pos_xm':'start_waypoint'})
            
            # self-introduce
            self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng'
            StateMachine.add('SELF_INTRO',
                                Speak(),
                                remapping ={'sentences':'sentences_2'},
                                transitions ={'succeeded':'succeeded','aborted':'SELF_INTRO','error':'error'})
        
        # we have already identity the people from 0-4 when the first recongization
        self.sm_FaceDetect = StateMachine(outcomes = ['succeeded','aborted','error'],
                                            output_keys = ['people_position','num_list'])
        with self.sm_FaceDetect:

            
            self.sm_FaceDetect.userdata.people_position =list()
            
            self.sm_FaceDetect.userdata.sentences = 'please look at me'
            StateMachine.add('SPEAK',
                                Speak(),
                                remapping = {'sentences':"sentences"},
                                transitions = {'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
            # call face_reco service for get all the people position which is a list
            self.sm_FaceDetect.userdata.name_id =-1
            self.sm_FaceDetect.userdata.num_list = list()
            StateMachine.add('GET_POSITION',
                                FaceReco(),
                                remapping  ={'name_id':'name_id','position':'people_position','num_list':'num_list'},
                                transitions ={'succeeded':'succeeded',
                                              'again':'GET_POSITION',
                                              'aborted':'GET_POSITION',
                                              'error':'error',
                                              'turn_l':'TURN_L',
                                              'turn_r':'TURN_R',
                                              'train_error':'aborted'})

            # if the face-recognize failed, we should make some remedy to make the state continue
            self.sm_FaceDetect.userdata.turn_point_1 = Point(0.0,0.0,pi/8)                   
            StateMachine.add('TURN_L',
                                SimpleMove_move(),
                                transitions={'succeeded':'SPEAK_2','error':'error'},
                                remapping ={'point':'turn_point_1'})
            
            self.sm_FaceDetect.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
            StateMachine.add('TURN_R',
                                SimpleMove_move(),
                                remapping ={'point':'turn_point_2'},
                                transitions ={'succeeded':'SPEAK_2','error':'error'})
            StateMachine.add('SPEAK_2',
                                Speak(),
                                remapping ={'sentences':'sentences'},
                                transitions ={'succeeded':'GET_POSITION','aborted':'aborted','error':'error'})
        
        self.sm_GetTarget = StateMachine(outcomes =['succeeded','aborted','error'],
                                            input_keys =['target'])#the target is a string.....
        with self.sm_GetTarget:

            # because xm is nav to pose in the nav_pose 
            self.sm_GetTarget.userdata.nav_ps = self.waypoints[1]
            # this smach code donnot to grasp ,so this part is useless
            StateMachine.add('NAV_TARGET',
                                NavStack(),
                                remapping ={'pos_xm':'nav_ps'},
                                transitions ={'succeeded':'FIND_OBJECT','aborted':'NAV_TARGET','error':'error'})
            self.sm_GetTarget.userdata.object_pos = PointStamped()
            StateMachine.add('FIND_OBJECT',
                                new_vision.FindObject(),
                                remapping ={'name':'target','object_pos':'object_pos'},
                                transitions ={'succeeded':'TALK','aborted':'FIND_OBJECT','error':'error'})
            self.sm_GetTarget.userdata.sentences = 'I find the target'
            StateMachine.add('TALK',
                                Speak(),
                                remapping ={'sentences':'sentences'},
                                transitions ={'succeeded':'PICK','aborted':'TALK','error':'error'})
            self.sm_GetTarget.userdata.arm_mode_1 =1
            StateMachine.add('PICK',
                                ArmCmd(),
                                remapping ={'mode':'arm_mode_1','arm_ps':'object_pos'},
                                transitions ={'succeeded':'succeeded','aborted':'succeeded','error':'error'})
            self.sm_GetTarget.userdata.arm_mode_2 = 0
            self.sm_GetTarget.userdata.arm_ps_2 = PointStamped()
            StateMachine.add('PUT',
                                PlaceBag(),
                                transitions ={'succeeded':'succeeded','aborted':'aborted'})
        
        # concurrence for the speech_node
        # we want to make the speech-meaning a timeout, so we use the concurrence function
        # but in fact we can also use multithread to slove the problem 
        self.meta_Remember = Concurrence(outcomes=['succeeded','aborted','error'],
                                            output_keys=['name','target'],
                                            default_outcome ='succeeded',
                                            outcome_map={'succeeded':{'NAME_AND_THING':'succeeded'}},
                                            child_termination_cb =self.child_cb)
        with self.meta_Remember:
            Concurrence.add('GET_EMPTY_NAME',
                                Hehe()
                                )
            
            Concurrence.add('NAME_AND_THING',
                                NameAndThing(),
                                remapping ={'name':'name','target':'target'}
                                )


        # input  one position is a list
        # io_keys can wirte and read 
        # userdata can update real-time 
        # no need to call the cv service
        self.sm_Remember = StateMachine(outcomes =['succeeded','aborted','error'],
                                        input_keys =['person_position'],
                                        output_keys =['name','target']
                                        )
        with self.sm_Remember:
            # rospy.logerr("sm_Remember add state")
            # self.sm_Remember.userdata.nav_mode =1
            # rospy.logerr("Is here1?")
            
            StateMachine.add('NAV_GO',
                                NavStack(),
                                remapping ={'pos_xm':'person_position'},
                                transitions ={'succeeded':'TALK','aborted':'NAV_GO','error':'error'}
                                )
                                
            self.sm_Remember.userdata.sentences = "what is your name and what do you want"
            StateMachine.add('TALK',
                                Speak(),
                                remapping ={'sentences':'sentences'},
                                transitions ={'succeeded':'GET_BOTH','aborted':'TALK','error':'error'})
            
            # StateMachine.add('RUNNODE',
            #                     RunNode(),
            #                     transitions ={'succeeded':'GET_BOTH','aborted':'aborted'})             
            StateMachine.add('GET_BOTH',
                                self.meta_Remember,
                                remapping ={'name':'name','target':'target'},
                                transitions ={'succeeded':'WAIT','aborted':'GET_BOTH','error':'error'})
            self.sm_Remember.userdata.rec =2.0
            StateMachine.add('WAIT',
                                Wait(),
                                remapping={'rec':'rec'},
                                transitions ={'succeeded':'succeeded','error':"error"})
           
        
        self.sm_GiveBack = StateMachine(outcomes =['succeeded','aborted','error'],
                                            input_keys =['name_id','goal_name','goal_target'])# the name is a string
        with self.sm_GiveBack:
            # rospy.logerr('sm_GiveBack add State')
            
            # self.sm_GiveBack.userdata.nav_ps = self.waypoints[0]   
            # self.sm_GiveBack.userdata.nav_mode_1 =0     
            # StateMachine.add('NAV_ROOM',
            #                     NavStack(),
            #                     remapping ={'pos_xm':'nav_ps','mode':'nav_mode_1'},
            #                     transitions ={'succeeded':'FACE_RECO','aborted':'NAV_ROOM','error':'error'})
            # self.sm_GiveBack.userdata.name_id =0
            # self.sm_GiveBack.userdata.name_list =list()
            self.sm_GiveBack.userdata.sentences = "please look at me"
            StateMachine.add('SPEAK',
                                Speak(),
                                remapping ={'sentences':'sentences'},
                                transitions ={'succeeded':'WAIT','aborted':'aborted','error':'error'})
            self.sm_GiveBack.userdata.rec =5.0
            StateMachine.add('WAIT',
                                Wait(),
                                remapping ={'rec':'rec'},
                                transitions ={'succeeded':'FACE_RECO','error':'error'})
            self.sm_GiveBack.userdata.person_position =PointStamped()
            StateMachine.add('FACE_RECO',
                                FaceReco(),
                                remapping ={'position':'person_position','name_id':'name_id'},
                                transitions ={'succeeded':'NAV_GO',
                                              'again':'FACE_RECO',
                                              'aborted':'FACE_RECO',
                                              'error':'error',
                                              'turn_l':'TURN_L',
                                              'turn_r':'TURN_R',
                                              'train_error':'aborted'}
                                )
            self.sm_GiveBack.userdata.turn_point_1 = Point(0.0,0.0,pi/8)                   
            StateMachine.add('TURN_L',
                                SimpleMove_move(),
                                transitions={'succeeded':'SPEAK_2','error':'error'},
                                remapping ={'point':'turn_point_1'})
            
            self.sm_GiveBack.userdata.turn_point_2 = Point(0.0,0.0,-pi/8)
            StateMachine.add('TURN_R',
                                SimpleMove_move(),
                                remapping ={'point':'turn_point_2'},
                                transitions ={'succeeded':'SPEAK_2','error':'error'})

            StateMachine.add('SPEAK_2',
                                Speak(),
                                remapping ={'sentences':'sentences'},
                                transitions ={'succeeded':'FACE_RECO','aborted':'aborted','error':'error'})
            #please pay attention! 
            # self.sm_GiveBack.userdata.nav_mode_2 =1     
            StateMachine.add('NAV_GO',
                                NavStack(),
                                remapping ={'pos_xm':'person_position'},
                                transitions ={'succeeded':'GET_NAME','aborted':'NAV_GO','error':'error'})
            StateMachine.add("GET_NAME",
                                NameHehe2(),
                                remapping ={'goal_target':'goal_name','goal_name':'goal_name','goal_target':'goal_target', 'sentences':'sentences_1'},
                                transitions ={'succeeded':'TALK_1','aborted':'aborted','error':'error'})
            self.sm_GiveBack.userdata.sentences_1 =''
            StateMachine.add('TALK_1',
                                Speak(),
                                remapping ={'sentences':"sentences_1"},
                                transitions ={'succeeded':'PUT','aborted':"PUT",'error':'error'})

            StateMachine.add('PUT' , PlaceBag(),
                                transitions = {'succeeded':'succeeded',
                                                'aborted':'succeeded'})
            # this pose should be specified to make the people get it
            # self.sm_GiveBack.userdata.arm_mode =2
            # self.sm_GiveBack.userdata.place_ps = PointStamped()
            # self.sm_GiveBack.userdata.place_ps.header.frame_id ='base_footprint'
            # self.sm_GiveBack.userdata.place_ps.point.x =0.7
            # self.sm_GiveBack.userdata.place_ps.point.y =0.0
            # self.sm_GiveBack.userdata.place_ps.point.z =0.3
            # StateMachine.add('PLACE',
            #                     ArmCmd(),
            #                     remapping ={'mode':'arm_mode','arm_ps':'place_ps'},
            #                     transitions ={'succeeded':'TALK_2','aborted':'PLACE','error':'error'})
            # self.sm_GiveBack.userdata.sentences_2 = 'I get the thing you want, am I?'
            # StateMachine.add('TALK_2',
            #                 Speak(),
            #                 remapping ={'sentences':'sentences_2'},
            #                 transitions ={'succeeded':'succeeded','aborted':"aborted",'error':'error'})
            


        self.sm_EndTask = StateMachine(outcomes =['succeeded','aborted','error'])
        with self.sm_EndTask:
            # rospy.logerr('sm_EndTask add State')
            
            self.sm_EndTask.userdata.nav_ps = self.waypoints[2] 
            # self.sm_EndTask.userdata.nav_mode = 0     
            StateMachine.add('NAV_BYEBYE',
                                NavStack(),
                                remapping ={'pos_xm':'nav_ps'},
                                transitions ={'succeeded':'succeeded','aborted':'NAV_BYEBYE','error':'error'})
            self.sm_EndTask.userdata.point = Point(1.8,0.0,0.0)
            # StateMachine.add('SIMPLE_MOVE',
            #                     SimpleMove_move(),
            #                     remapping={'point':'point'},
            #                     transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})

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

            
            # we can generate some state which contant the list_value
            StateMachine.add('ENTERROOM',
                                self.sm_EnterRoom,
                                transitions ={'succeeded':'FACEDETECT','aborted':'aborted','error':'error'})
            self.WHOISWHO.userdata.people_position =list()
            self.WHOISWHO.userdata.num_list = list()
            StateMachine.add('FACEDETECT',
                                self.sm_FaceDetect,
                                remapping ={'people_position':'people_position','num_list':'num_list'},
                                transitions = {'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
                                )
            self.WHOISWHO.userdata.person_position = PointStamped()
            # if someone is recongized failed, we should make the total list in the same order
            # get the last element of the list

            # FGM: This state is to get different people pos
            StateMachine.add('GETPERSON',
                                GetValue(),
                                remapping ={'element_list':'people_position','element':'person_position'},
                                transitions ={'succeeded':'REMEMBER','aborted':"GETID",'error':'error'}
                                )
            # the cv will remember the face with the increasing ID
            self.WHOISWHO.userdata.name =''
            self.WHOISWHO.userdata.target= ''
            self.WHOISWHO.userdata.name_list =list()
            self.WHOISWHO.userdata.target_list =list()
            self.WHOISWHO.userdata.goal_name =''
            self.WHOISWHO.userdata.goal_target''
            # if donnot need to remember, the order of ids should be the same with the positions return 
            # the first of the position should be the 0
            # last be the 4
            
            StateMachine.add('REMEMBER',
                                self.sm_Remember,
                                remapping ={'person_position':'person_position','name':'name','target':'target'},
                                transitions ={'succeeded':'NAMEINLIST','aborted':'aborted','error':'error'}
                                )
            
            #this state use for joining the name and the target into the name_list and target_list 
            # insert in the head of the lists
            # if the name and target are empty, we should also append them to the lists
            StateMachine.add('NAMEINLIST',
                                NameInList(),
                                remapping ={'name':'name','target':'target','name_list':'name_list','target_list':'target_list'},
                                transitions ={'succeeded':'GETPERSON','aborted':'aborted','error':'error'}
                                )
            # ############################################################################
            # so far here ,the tasks of remember is completed , the rest is the tasks to return the target to the people
            # we should take out the name and the matched target for xm to grasp and give back
            # in fact, we will pop the name and target out the list,so the name_id is gradually reduced
            # ############################################################################
            StateMachine.add('GETTARGET',
                                GetValue2(),
                                remapping ={'element_list':'target_list','element':'target'},
                                transitions ={'succeeded':'CATCHTARGET','aborted':'ENDTASK','error':'error'})
            StateMachine.add('CATCHTARGET',
                                self.sm_GetTarget,
                                transitions= {'succeeded':'NAV_ROOM','aborted':'NAV_ROOM','error':'error'},
                                remapping = {'target':'target'})
            # ############################################################################
            self.WHOISWHO.userdata.nav_ps = self.waypoints[0]
            # self.WHOISWHO.userdata.nav_mode_1 =0
            StateMachine.add('NAV_ROOM',
                                NavStack(),
                                remapping ={'pos_xm':'nav_ps'},
                                transitions ={'succeeded':'GIVEBACK','aborted':'NAV_ROOM','error':'error'})
            StateMachine.add('CHECKFINISH',
                                CBState(self.checkfinish,outcomes=['finish','continue'],input_keys=['num_list']),
                                transitions={'finish':'ENDTASK',
                                             'continue':'GETTARGET'},
                                remapping={'num_list':'num_list'})
            # StateMachine.add('CLEAR_MAP',
            #                     ClearMap(),
            #                     transitions ={'succeeded':'GETID','aborted':'GETID'})
            # get the last num of the list, ID
            # if the name and target list is empty, will skipped this item
            self.WHOISWHO.userdata.sentences_1 = 'please change the order'
            StateMachine.add('SPEAK_1',
                                Speak(),
                                remapping ={'sentences':'sentences_1'},
                                transitions ={'succeeded':'WAIT_HEHE','aborted':'WAIT_HEHE','error':'error'})
            self.WHOISWHO.userdata.rec_hehe =10.0
            StateMachine.add('WAIT_HEHE',
                                Wait(),
                                remapping ={'rec':'rec_hehe'},
                                transitions ={'succeeded':'SPEAK_2','error':'error'})
            self.WHOISWHO.userdata.sentences_2 = 'I will make the recongization task'
            StateMachine.add('SPEAK_2',
                                Speak(),
                                transitions ={'succeeded':'GETID','aborted':'GETID','error':'error'},
                                remapping ={'sentences':'sentences_2'})

            self.WHOISWHO.userdata.name_id = 0   
            StateMachine.add('GETID',
                                GetId(),
                                remapping ={'output_id':'name_id','input_list':'name_list','num_list':'num_list'},
                                transitions ={'succeeded':'GETTARGET','aborted':'ENDTASK','error':'error'}
                                )
            StateMachine.add('GIVEBACK',
                                self.sm_GiveBack,
                                remapping ={'name_id':'name_id','name_list':'name_list','target_list':'target_list'},
                                transitions ={'succeeded':'CHECKFINISH','aborted':'CHECKFINISH','error':'error'}
                                )
            StateMachine.add('ENDTASK',
                                self.sm_EndTask,
                                transitions ={'succeeded':'succeeded','aborted':'aborted','error':'error'})

            # rospy.logerr('sm_Top execute begin')

            intro_server = IntrospectionServer('whoiswho',self.WHOISWHO, 'SM_ROOT')
            intro_server.start()
            out = self.WHOISWHO.execute()
            intro_server.stop()
            if out == 'succeeded':
                self.smach_bool = True
Пример #39
0
    def __init__(self):
        rospy.init_node('carry_smach_move', 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 = (0.0, 3*pi/2, pi/2, 3*pi/2, pi/2, 3*pi/2, 3*pi/2, 0.0, pi, 0.0, pi, 3*pi/2)

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

        # 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, -3.0, 0.0), quaternions[0]))
        self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1]))
        self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2]))
        self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3]))
        self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4]))
        self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5]))
        self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6]))
        self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7]))
        self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8]))
        self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9]))
        self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10]))
        self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11]))



	# State machine  
        #self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted'])
        #self.sm_out_main.userdata.goal = self.waypoints[2];

        #with self.sm_out_main:
	#   StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(),
        #                     transitions={'succeeded':'succeeded',
        #                                  'aborted':'aborted'}) 

	# Concurrent State machine 
        self.sm_in_main_room = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_sleep'],
					default_outcome='succeeded',
                                        child_termination_cb=self.in_main_room_child_termination_cb,
                                        outcome_cb=self.in_main_room_outcome_cb)
	self.sm_in_main_room.userdata.goal = self.waypoints[1];

        with self.sm_in_main_room:
	    Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
	    Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
	    Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
	    #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1]))


        # Concurrent State machine 
        self.sm_sleep = Concurrence(outcomes=['succeeded','aborted','preempted','go_kitchen','go_bedroom','go_main_room'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_sleep_child_termination_cb,
                                        outcome_cb=self.in_sleep_outcome_cb)
        self.sm_sleep.userdata.goal = self.waypoints[0];

        with self.sm_sleep:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0]))

	# Concurrent State machine 
        self.sm_in_kitchen = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_bedroom','go_sleep'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_kitchen_child_termination_cb,
                                        outcome_cb=self.in_kitchen_outcome_cb)
        self.sm_in_kitchen.userdata.goal = self.waypoints[6];

        with self.sm_in_kitchen:
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
            #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6]))

        # Concurrent State machine 
        self.sm_in_bedroom = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_kitchen','go_sleep'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_bedroom_child_termination_cb,
                                        outcome_cb=self.in_bedroom_outcome_cb)
        self.sm_in_bedroom.userdata.goal = self.waypoints[11];

        with self.sm_in_bedroom:
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb))
            #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11]))




	# 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('IN_MAIN_ROOM', self.sm_in_main_room, transitions={'succeeded':'IN_MAIN_ROOM', 
										'go_kitchen':'NAV_M2K_M',
										'go_sleep':'IN_SLEEP',
										'go_bedroom':'NAV_M2B_M'})
            StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={'succeeded':'succeeded', 
                                                                                'go_main_room':'NAV_K2M_K',
                                                                                'go_sleep':'NAV_K2S_K',
                                                                                'go_bedroom':'NAV_K2B_K'})
            StateMachine.add('IN_BEDROOM', self.sm_in_bedroom, transitions={'succeeded':'succeeded', 
                                                                                'go_kitchen':'NAV_B2K_B',
                                                                                'go_sleep':'NAV_B2S_B',
                                                                                'go_main_room':'NAV_B2M_B'})
            StateMachine.add('IN_SLEEP', self.sm_sleep, transitions={'succeeded':'succeeded', 
                                                                                'go_kitchen':'NAV_S2K_M',
                                                                                'go_main_room':'IN_MAIN_ROOM',
                                                                                'go_bedroom':'NAV_S2B_M'})
	    StateMachine.add('NAV_M2K_M', Nav2Waypoint(self.waypoints[2]),
                             transitions={'succeeded':'NAV_M2K_K',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_M2K_K', Nav2Waypoint(self.waypoints[5]),
                             transitions={'succeeded':'NAV_M2K_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_M2K_END', Nav2Waypoint(self.waypoints[6]),
                             transitions={'succeeded':'IN_KITCHEN',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_K2M_K', Nav2Waypoint(self.waypoints[4]),
                             transitions={'succeeded':'NAV_K2M_M',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2M_M', Nav2Waypoint(self.waypoints[3]),
                             transitions={'succeeded':'NAV_K2M_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2M_END', Nav2Waypoint(self.waypoints[1]),
                             transitions={'succeeded':'IN_MAIN_ROOM',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_M2B_M', Nav2Waypoint(self.waypoints[2]),
                             transitions={'succeeded':'NAV_M2B_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_M2B_C', Nav2Waypoint(self.waypoints[8]),
                             transitions={'succeeded':'NAV_M2B_B',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_M2B_B', Nav2Waypoint(self.waypoints[10]),
                             transitions={'succeeded':'NAV_M2B_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_M2B_END', Nav2Waypoint(self.waypoints[11]),
                             transitions={'succeeded':'IN_BEDROOM',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_K2S_K', Nav2Waypoint(self.waypoints[4]),
                             transitions={'succeeded':'NAV_K2S_M',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2S_M', Nav2Waypoint(self.waypoints[3]),
                             transitions={'succeeded':'NAV_K2S_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2S_END', Nav2Waypoint(self.waypoints[0]),
                             transitions={'succeeded':'IN_SLEEP',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_K2B_K', Nav2Waypoint(self.waypoints[4]),
                             transitions={'succeeded':'NAV_K2B_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2B_C', Nav2Waypoint(self.waypoints[8]),
                             transitions={'succeeded':'NAV_K2B_B',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2B_B', Nav2Waypoint(self.waypoints[10]),
                             transitions={'succeeded':'NAV_K2B_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_K2B_END', Nav2Waypoint(self.waypoints[11]),
                             transitions={'succeeded':'IN_BEDROOM',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_B2K_B', Nav2Waypoint(self.waypoints[9]),
                             transitions={'succeeded':'NAV_B2K_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2K_C', Nav2Waypoint(self.waypoints[7]),
                             transitions={'succeeded':'NAV_B2K_K',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2K_K', Nav2Waypoint(self.waypoints[5]),
                             transitions={'succeeded':'NAV_B2K_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2K_END', Nav2Waypoint(self.waypoints[6]),
                             transitions={'succeeded':'IN_KITCHEN',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_B2S_B', Nav2Waypoint(self.waypoints[9]),
                             transitions={'succeeded':'NAV_B2S_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2S_C', Nav2Waypoint(self.waypoints[7]),
                             transitions={'succeeded':'NAV_B2S_M',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2S_M', Nav2Waypoint(self.waypoints[3]),
                             transitions={'succeeded':'NAV_B2S_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2S_END', Nav2Waypoint(self.waypoints[0]),
                             transitions={'succeeded':'IN_SLEEP',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_B2M_B', Nav2Waypoint(self.waypoints[9]),
                             transitions={'succeeded':'NAV_B2M_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2M_C', Nav2Waypoint(self.waypoints[7]),
                             transitions={'succeeded':'NAV_B2M_M',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2M_M', Nav2Waypoint(self.waypoints[3]),
                             transitions={'succeeded':'NAV_B2M_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_B2M_END', Nav2Waypoint(self.waypoints[1]),
                             transitions={'succeeded':'IN_MAIN_ROOM',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_S2B_M', Nav2Waypoint(self.waypoints[2]),
                             transitions={'succeeded':'NAV_S2B_C',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_S2B_C', Nav2Waypoint(self.waypoints[8]),
                             transitions={'succeeded':'NAV_S2B_B',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_S2B_B', Nav2Waypoint(self.waypoints[10]),
                             transitions={'succeeded':'NAV_S2B_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_S2B_END', Nav2Waypoint(self.waypoints[11]),
                             transitions={'succeeded':'IN_BEDROOM',
                                          'aborted':'aborted'})

            StateMachine.add('NAV_S2K_M', Nav2Waypoint(self.waypoints[2]),
                             transitions={'succeeded':'NAV_S2K_K',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_S2K_K', Nav2Waypoint(self.waypoints[5]),
                             transitions={'succeeded':'NAV_S2K_END',
                                          'aborted':'aborted'})
            StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]),
                             transitions={'succeeded':'IN_KITCHEN',
                                          'aborted':'aborted'})





        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_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()
Пример #40
0
class SMACHAI():
    def __init__(self):
        rospy.init_node('HOME_automation_smach', 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]))



	# State machine for light entry 
        self.sm_light_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_light_entry.userdata.day_mode = 1;

        with self.sm_light_entry:
            StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb),
					  transitions={'valid':'LOOK_ENTRY',
					  'invalid':'LIGHT_UP'})
            StateMachine.add('LIGHT_UP', LightEntry(),
                             transitions={'succeeded':'succeeded'})

        # State machine for dark entry 
        self.sm_dark_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_dark_entry.userdata.day_mode = 1;

        with self.sm_dark_entry:
            StateMachine.add('LOOK_ENTRY_OFF', MonitorState("/HOME/entry_noOne", Empty, self.empty_cb),
                                          transitions={'valid':'LOOK_ENTRY_OFF',
                                          'invalid':'LIGHT_DOWN'})
            StateMachine.add('LIGHT_DOWN', DarkEntry(),
                             transitions={'succeeded':'succeeded'})


	# State machine for day mode
        self.sm_day_mode = Concurrence(outcomes=['succeeded','aborted','preempted','go_shower','go_sleep','go_eat','go_out'],
					default_outcome='succeeded',
                                        child_termination_cb=self.daymode_child_termination_cb,
                                        outcome_cb=self.daymode_outcome_cb)
	self.sm_day_mode.userdata.day_mode = 1;

        with self.sm_day_mode:
	    Concurrence.add('LOOK_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb))
	    Concurrence.add('LOOK_LEAVING', MonitorState("/HOME/leaving_home", Empty, self.empty_cb))
	    Concurrence.add('LOOK_SLEEP', MonitorState("/HOME/go_sleep", Empty, self.empty_cb))
	    Concurrence.add('LOOK_EAT', MonitorState("/HOME/go_eat", Empty, self.empty_cb))
	    Concurrence.add('LOOK_ENTRY', self.sm_light_entry)
	    Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)


        # State machine for leaving home
        self.sm_leaving_home = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_leaving_home:
            StateMachine.add('LEAV', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})

        # State machine for going to sleep
        self.sm_going_sleep = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_going_sleep:
            StateMachine.add('GOING_SLEEP', GoingSleep(),
                             transitions={'succeeded':'succeeded'})


	# State machine for day mode
        self.sm_wait_bed = Concurrence(outcomes=['succeeded','aborted','preempted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.useless_child_termination_cb,
                                        outcome_cb=self.useless_outcome_cb)

        with self.sm_wait_bed:
            Concurrence.add('LOOK_INBED', MonitorState("/METAWATCH/button2", Empty, self.empty_cb))
            Concurrence.add('TIMEOUT', TimeoutToBed())


	# State machine for in bed
        self.sm_in_bed = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_in_bed:
            StateMachine.add('IN_BED', InBed(),
                             transitions={'succeeded':'succeeded'})



	# State machine for light entry 
        self.sm_lightn_entry = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_lightn_entry.userdata.day_mode = 0;

        with self.sm_lightn_entry:
            StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb),
                                          transitions={'valid':'LOOK_ENTRY',
                                          'invalid':'LIGHT_UP'})
            StateMachine.add('LIGHT_UP', LightEntry(),
                             transitions={'succeeded':'succeeded'})


	# State machine for night mode
        self.sm_night_mode = Concurrence(outcomes=['succeeded','aborted','preempted','wake_up'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.nightmode_child_termination_cb,
                                        outcome_cb=self.nightmode_outcome_cb)
        self.sm_night_mode.userdata.day_mode = 0;

        with self.sm_night_mode:
            Concurrence.add('LOOK_WAKE', MonitorState("/HOME/wake_up", Empty, self.empty_cb))
            Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry)
            Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry)


        # State machine for night mode
        #self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted'])
	#self.sm_night_mode.userdata.day_mode = 0;

        #with self.sm_night_mode:
        #    StateMachine.add('NIGHT_MOD', Pause(),
        #                     transitions={'succeeded':'succeeded',
        #                                  'aborted':'aborted'})

        # State machine for waking up 
        self.sm_waking_up = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_waking_up.userdata.day_mode = 0;

        with self.sm_waking_up:
            StateMachine.add('WAKING_UP', WakingUp(),
                             transitions={'succeeded':'succeeded'})

	# State machine for waking up 
        self.sm_going_eat = StateMachine(outcomes=['succeeded','aborted','preempted'])
        self.sm_going_eat.userdata.day_mode = 1;

        with self.sm_going_eat:
            StateMachine.add('EATTTTTTTT', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})





	# State machine for home
        self.sm_home = StateMachine(outcomes=['succeeded','aborted','preempted','going_out'])

        with self.sm_home:
            StateMachine.add('DAY_MODE', self.sm_day_mode,
                             transitions={'succeeded':'DAY_MODE',
					  'go_shower':'PREPARING_SHOWER',
                                          'go_sleep':'GOING_SLEEP',
                                          'go_eat':'GOING_EAT',
                                          'go_out':'LEAVING_HOME',
                                          'aborted':'aborted'})
            StateMachine.add('PREPARING_SHOWER', PreparingShower(),
                             transitions={'succeeded':'GO_SHOWER',
                                          'aborted':'aborted'})
            StateMachine.add('GO_SHOWER', GoShower(),
                             transitions={'succeeded':'STOP_SHOWER',
                                          'aborted':'aborted'})
            StateMachine.add('STOP_SHOWER', StopShower(),
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('LEAVING_HOME', self.sm_leaving_home,
                             transitions={'succeeded':'going_out',
                                          'aborted':'aborted'})
            StateMachine.add('GOING_SLEEP', self.sm_going_sleep,
                             transitions={'succeeded':'WAIT_TO_BED',
                                          'aborted':'aborted'})
	    StateMachine.add('WAIT_TO_BED', self.sm_wait_bed,
                             transitions={'succeeded':'IN_BED',
					  'preempted':'IN_BED',
                                          'aborted':'aborted'})
            StateMachine.add('IN_BED', self.sm_in_bed,
                             transitions={'succeeded':'NIGHT_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('NIGHT_MODE', self.sm_night_mode,
                             transitions={'succeeded':'NIGHT_MODE',
					  'wake_up':'WAKING_UP',
                                          'aborted':'aborted'})
            StateMachine.add('WAKING_UP', self.sm_waking_up,
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})
            StateMachine.add('GOING_EAT', self.sm_going_eat,
                             transitions={'succeeded':'DAY_MODE',
                                          'aborted':'aborted'})






	# State machine for waking up 
        self.sm_guarding = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with self.sm_guarding:
            StateMachine.add('GUARD', Pause(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'aborted'})



	# State machine with concurrence
        self.sm_incoming_home = Concurrence(outcomes=['succeeded', 'aborted'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.incoming_child_termination_cb,
                                        outcome_cb=self.incoming_outcome_cb)

        # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine             
        with self.sm_incoming_home:
           Concurrence.add('LOOK_CONNECTION', MonitorState("/METAWATCH/connected", Empty, self.empty_cb))
           Concurrence.add('LOOK_ENTERING', MonitorState("/HOME/entry_door_open", Empty, self.empty_cb))





        # State machine for away
        self.sm_away = StateMachine(outcomes=['succeeded','aborted','preempted','entering_home'])

        with self.sm_away:
            StateMachine.add('GUARDING_MODE', self.sm_guarding,
                             transitions={'succeeded':'INCOMING_HOME',
                                          'aborted':'aborted'})
            StateMachine.add('INCOMING_HOME', self.sm_incoming_home,
                             transitions={'succeeded':'entering_home',
                                          'aborted':'aborted'})




        # 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('AT_HOME', self.sm_home, transitions={'succeeded':'AT_HOME', 'going_out':'AWAY'})
            #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
            StateMachine.add('AWAY', self.sm_away, transitions={'succeeded':'AWAY', 'entering_home':'AT_HOME'})








        # 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 empty_cb(self, userdata, msg):
	#rospy.loginfo("Empty message received.")
        return False

    # Gets called when ANY child state terminates
    def useless_child_termination_cb(self, outcome_map):
	rospy.loginfo("useless_child_termination_cb.")
        return True


    # Gets called when ALL child states are terminated
    def useless_outcome_cb(self, outcome_map):
        rospy.loginfo("useless_outcome_cb.")
        return 'succeeded'


    # Gets called when ANY child state terminates
    def daymode_child_termination_cb(self, outcome_map):
	#rospy.loginfo("daymode_child_termination_cb.")
	return True


    # Gets called when ALL child states are terminated
    def daymode_outcome_cb(self, outcome_map):
        #rospy.loginfo("daymode_outcome_cb.")
	# If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['LOOK_SHOWER'] == 'invalid':
            rospy.loginfo("Going to shower ")
            return 'go_shower'
        if outcome_map['LOOK_LEAVING'] == 'invalid':
            rospy.loginfo("Leaving home")
            return 'go_out'
        if outcome_map['LOOK_SLEEP'] == 'invalid':
            rospy.loginfo("Going to sleep")
            return 'go_sleep'
        if outcome_map['LOOK_EAT'] == 'invalid':
            rospy.loginfo("Going to eat")
            return 'go_eat'
        if outcome_map['LOOK_ENTRY'] == 'succeeded':
            rospy.loginfo("Restart looking entry")
            return 'succeeded'
        if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded':
            rospy.loginfo("No one in the entry")
            return 'succeeded'
        else:
            return 'aborted'


    # Gets called when ANY child state terminates
    def nightmode_child_termination_cb(self, outcome_map):
        #rospy.loginfo("daymode_child_termination_cb.")
        return True


    # Gets called when ALL child states are terminated
    def nightmode_outcome_cb(self, outcome_map):
        #rospy.loginfo("daymode_outcome_cb.")
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['LOOK_WAKE'] == 'invalid':
            rospy.loginfo("Wake up dude !")
            return 'wake_up'
        if outcome_map['LOOK_ENTRY'] == 'succeeded':
            rospy.loginfo("Restart looking entry")
            return 'succeeded'
        if outcome_map['LOOK_ENTRY_OFF'] == 'succeeded':
            rospy.loginfo("No one in the entry")
            return 'succeeded'
        else:
            return 'aborted'

    # Gets called when ANY child state terminates
    def incoming_child_termination_cb(self, outcome_map):
        # If the current navigation task has succeeded, return True
        if outcome_map['LOOK_CONNECTION'] == 'succeeded':
            rospy.loginfo("MW connected. Welcome back.")
            return True
        # If the MonitorState state returns False (invalid), store the current nav goal and recharge
        if outcome_map['LOOK_ENTERING'] == 'succeeded':
            rospy.loginfo("Someone entering...")
            return True
        else:
            return False


    # Gets called when ALL child states are terminated
    def incoming_outcome_cb(self, outcome_map):
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['LOOK_CONNECTION'] == 'succeeded':
            rospy.loginfo("MW connected. Welcome back ")
            return 'succeeded'
        if outcome_map['LOOK_ENTERING'] == 'succeeded':
            rospy.loginfo("Someone entering..")
            return 'succeeded'
        else:
            return 'aborted'







    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 home automation...")
        
        self.sm_day_mode.request_preempt()
        
        rospy.sleep(1)
Пример #41
0
    def __init__(self):
        rospy.init_node('Shopping')
        self.smach_bool = False
        rospy.on_shutdown(self.shutdown)

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

        with self.sm_EnterRoom:
            StateMachine.add('DOOR_DETECT',
                             DoorDetect().door_detect_,
                             transitions={
                                 'invalid': 'WAIT',
                                 'valid': 'DOOR_DETECT',
                                 'preempted': 'aborted'
                             })

            # waits
            StateMachine.add('WAIT',
                             Wait(),
                             transitions={
                                 'succeeded': 'ENTER',
                                 'error': 'error'
                             },
                             remapping={'rec': 'wait_len'})

            # StateMachine.add('ENTER', LinearDisplacement(), transitions={'succeeded': 'succeeded',
            #                                                              'preempted': 'ENTER',
            #                                                              'error': 'error'},
            #                  remapping={'displacementVec': 'point'})

            self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][
                'pos']
            StateMachine.add('ENTER',
                             NavStack(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'ENTER',
                                 'error': 'error'
                             },
                             remapping={'pos_xm': 'start_waypoint'})

        # how to get stop signal?
        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:
                self.meta_follow.userdata.pos_xm = Pose()
                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',
                            CheckStop2(),
                            remapping={
                                'PT_LIST': 'PT_LIST',
                                'mission': 'mission'
                            })

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

            self.Pick.userdata.target_pos = PointStamped()
            self.Pick.userdata.nav_pos = Pose()
            self.Pick.userdata.pick_pos = PointStamped()
            self.Pick.userdata.distance = 1.0
            self.Pick.userdata.distance2 = 0.9
            self.Pick.userdata.target_mode = 1
            self.Pick.userdata.objmode = 1
            #self.Pick.userdata.target = 'ice_tea'

            StateMachine.add('FIND',
                             FindObject(),
                             transitions={
                                 'succeeded': 'DISTANCE',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'name': 'target',
                                 'object_pos': 'target_pos',
                                 'object_map_point': 'object_map_point'
                             })
            #    StateMachine.add('FIND', self.FindObj,
            #                      transitions={'succeeded': 'DISTANCE',
            #                                   'aborted': 'aborted', 'error': 'error'},
            #                      remapping={'target': 'target',
            #                                 'indice': 'indice',
            #                                 'targets': 'targets',
            #                                 'object_map_point':'object_map_point',
            #                                 'target_pos': 'target_pos'})

            # StateMachine.add('FIND', self.FindObj,
            #                  transitions={'succeeded': 'DISTANCE',
            #                               'aborted': 'aborted', 'error': 'error'},
            #                  remapping = {'name':'target' ,
            #                                 'object_pos':'target_pos',
            #                                 'object_map_point':'object_map_point'})
            StateMachine.add('DISTANCE',
                             CBState(self.PickDistance,
                                     outcomes=['succeeded', 'error'],
                                     input_keys=['name'],
                                     output_keys=['distance']),
                             transitions={
                                 'succeeded': 'POS_JUS',
                                 'error': 'POS_JUS'
                             },
                             remapping={
                                 'distance': 'distance',
                                 'name': 'target'
                             })
            StateMachine.add('POS_JUS',
                             new_PosJustfy(),
                             transitions={
                                 'succeeded': 'NAV',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'pose': 'nav_pos',
                                 'distance': 'distance',
                                 'object_pos': 'target_pos'
                             })

            StateMachine.add('NAV',
                             NavStack(),
                             transitions={
                                 'succeeded': 'FIND_AGAIN',
                                 'aborted': 'NAV',
                                 'error': 'error',
                             },
                             remapping={'pos_xm': 'nav_pos'})

            StateMachine.add('PICK2',
                             ArmCmdForTf(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'error': 'error',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'arm_ps': 'object_map_point',
                                 'mode': 'objmode'
                             })

            StateMachine.add('FIND_AGAIN',
                             FindObject(),
                             transitions={
                                 'succeeded': 'PICK',
                                 'aborted': 'PICK2',
                                 'error': 'error'
                             },
                             remapping={
                                 'object_pos': 'pick_pos',
                                 'name': 'target',
                                 'object_state': 'object_state'
                             })

            StateMachine.add('PICK',
                             new_ArmCmd(),
                             transitions={
                                 'succeeded': 'succeeded',
                                 'error': 'error',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'arm_ps': 'pick_pos',
                                 'mode': 'objmode',
                                 'object_state': 'object_state'
                             })

        self.sm_FaceDetect = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'],
            output_keys=['people_position', 'num_list'])
        with self.sm_FaceDetect:

            self.sm_FaceDetect.userdata.people_position = list()

            self.sm_FaceDetect.userdata.sentences = 'please look at me'
            StateMachine.add('SPEAK',
                             Speak(),
                             remapping={'sentences': "sentences"},
                             transitions={
                                 'succeeded': 'GET_POSITION',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             })
            # call face_reco service for get all the people position which is a list
            self.sm_FaceDetect.userdata.name_id = -1  #
            self.sm_FaceDetect.userdata.num_list = list()
            self.sm_FaceDetect.userdata.distance = 0.6
            StateMachine.add('GET_POSITION',
                             FaceReco(),
                             remapping={
                                 'name_id': 'name_id',
                                 'position': 'people_position',
                                 'num_list': 'num_list',
                                 'distance': 'distance'
                             },
                             transitions={
                                 'succeeded': 'succeeded',
                                 'again': 'GET_POSITION',
                                 'aborted': 'GET_POSITION',
                                 'error': 'error',
                                 'turn_l': 'MOVEAHEAD',
                                 'turn_r': 'MOVEAHEAD',
                                 'train_error': 'aborted'
                             })
            self.sm_FaceDetect.userdata.point_1 = Point(0.1, 0.0, 0.0)
            StateMachine.add(
                'MOVEAHEAD',
                SimpleMove_move(),
                transitions={
                    'succeeded': 'GET_POSITION',
                    'error': 'error',
                    'aborted': 'GET_POSITION'
                },
                remapping={'point': 'point_1'},
            )

        self.GetTask = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                    output_keys=['task'])
        with self.GetTask:
            self.GetTask.userdata.people_position = list()
            self.GetTask.userdata.num_list = list()
            self.GetTask.userdata.person_position = Pose()

            StateMachine.add('GET_POSITION',
                             self.sm_FaceDetect,
                             transitions={
                                 'succeeded': 'GET_VALUE1',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={
                                 'people_position': 'people_position',
                                 'num_list': 'num_list'
                             })

            StateMachine.add('GET_VALUE1',
                             GetValue(),
                             remapping={
                                 'element_list': 'people_position',
                                 'element': 'person_position'
                             },
                             transitions={
                                 'succeeded': 'NAV1',
                                 'aborted': "succeeded",
                                 'error': 'error'
                             })

            StateMachine.add('NAV1',
                             NavStack(),
                             transitions={
                                 'succeeded': 'ASK_TASK1',
                                 'aborted': 'NAV1',
                                 'error': 'error'
                             },
                             remapping={'pos_xm': 'person_position'})

            self.GetTask.userdata.sentence1 = 'what do you want?'
            StateMachine.add('ASK_TASK1',
                             Speak(),
                             transitions={
                                 'succeeded': 'ANS1',
                                 'aborted': 'GET_VALUE1',
                                 'error': 'error'
                             },
                             remapping={'sentence': 'sentence1'})
            StateMachine.add('ANS1',
                             ShoppingGetTask(),
                             transitions={
                                 'succeeded': 'GET_VALUE1',
                                 'aborted': 'GET_VALUE1'
                             },
                             remapping={'task': 'task'})
            # StateMachine.add('ASK_TASK2' , Speak() , transitions={'succeeded':'ANS2',
            #                                                  'aborted':'aborted',
            #                                                  'error':'error'},
            #                                         remapping={'sentence':'sentence1'})
            # StateMachine.add('ANS2' , ShoppingGetTask() , transitions={'succeeded':'succeeded',
            #                                                             'aborted':'aborted'},
            #                                             remapping={'task':'task'})

        self.DealTask = StateMachine(
            outcomes=['succeeded', 'aborted', 'error'],
            input_keys=['task', 'mission'])

        with self.DealTask:
            self.DealTask.userdata.nav_pos = Pose()
            self.DealTask.userdata.indice = 0
            self.DealTask.userdata.indice2 = 3
            self.DealTask.userdata.name = ''

            StateMachine.add('NXT_TASK',
                             ShoppingNextTask(),
                             transitions={
                                 'go': 'NAV',
                                 'back': 'NAV_CASH',
                                 'finish': 'succeeded',
                                 'aborted': "aborted"
                             },
                             remapping={
                                 'pose': 'nav_pos',
                                 'name': 'name',
                                 'indice': 'indice'
                             })

            StateMachine.add('NAV',
                             NavStack(),
                             transitions={
                                 'succeeded': 'PICK',
                                 'aborted': 'NAV',
                                 'error': 'error'
                             },
                             remapping={'pos_xm', 'nav_pos'})

            StateMachine.add('PICK',
                             self.Pick,
                             transitions={
                                 'succeeded': 'NXT_TASK',
                                 'aborted': 'NXT_TASK',
                                 'error': 'error'
                             },
                             remapping={'target': 'name'})
            StateMachine.add('NAV_CASH',
                             NavStack(),
                             transitions={
                                 'succeeded': 'PLACE',
                                 'aborted': 'NAV_CASH',
                                 'error': 'error'
                             },
                             remapping={'pos_xm', 'nav_pos'})

            #place need to be  upgraded
            StateMachine.add('PLACE',
                             Place2(),
                             transitions={
                                 'succeeded': 'NXT_TASK',
                                 'aborted': 'NXT_TASK',
                             })

        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('ENTERROOM',
            #                     self.sm_EnterRoom,
            #                     transitions={'succeeded':'START','aborted':'aborted'})
            StateMachine.add('START',
                             GetSignal(),
                             transitions={
                                 'succeeded': 'RUNNODE',
                                 'aborted': 'aborted'
                             })
            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': 'TRACE',
                                 'stop': 'GET_TASK',
                                 'aborted': 'aborted'
                             },
                             remapping={
                                 'PT_LIST': 'PT_LIST',
                                 'mission': 'mission'
                             })
            StateMachine.add('GET_TASK',
                             self.GetTask,
                             transitions={
                                 'succeeded': 'DEAL_TASK',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={'task': 'task'})

            StateMachine.add('DEAL_TASK',
                             self.DealTask,
                             transitions={
                                 'succeeded': 'succeeded',
                                 'aborted': 'aborted',
                                 'error': 'error'
                             },
                             remapping={'task': 'task'})

        intro_server = IntrospectionServer('shopping', self.shopping,
                                           'SM_ROOT')
        intro_server.start()
        out = self.shopping.execute()
        intro_server.stop()
        if out == 'succeeded':
            self.smach_bool = True
Пример #42
0
def main():
    rospy.init_node('smach_usecase_step_06')

    # 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':'TELEPORT2'})

        # 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_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
            draw_monitor_cc = Concurrence(
                    ['succeeded','aborted','preempted'],
                    'aborted',
                    child_termination_cb = lambda so: True,
                    outcome_map = {
                        'succeeded':{'DRAW':'succeeded'},
                        'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
                        'aborted':{'MONITOR':'invalid'}})
            Concurrence.add('SMALL',draw_monitor_cc)
            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))

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

    # Set preempt handler
    smach.set_preempt_handler(sm0)

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

    # Signal handler
    rospy.spin()
Пример #43
0
    def __init__(self):
        rospy.init_node('help_me_carry')
        rospy.on_shutdown(self.shutdown)
        self.smach_bool = False
        self.trace = Concurrence(outcomes=['succeeded', 'aborted'],
                                 default_outcome='succeeded',
                                 outcome_map={'succeeded': {'STOP': 'stop'},
                                              'aborted': {'FOLLOW': 'aborted'}},
                                 #                                 outcome_cb = self.trace_out_cb,
                                 child_termination_cb=self.trace_child_cb)
        with self.trace:
            self.meta_follow = StateMachine(['succeeded', 'aborted'])
            with self.meta_follow:
                StateMachine.add('FOLLOW',
                                 SimpleFollow(),
                                 transitions={'succeeded': 'FOLLOW', 'aborted': 'aborted', 'preempt': 'succeeded'})

            Concurrence.add('FOLLOW', self.meta_follow)
            Concurrence.add('STOP', CheckStop())
            Concurrence.add('RUNNODE', RunNode())

        self.xm_Nav = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                   input_keys=['target', 'current_task'])
        with self.xm_Nav:
            # userdata 列表
            # xm的位置信息,由GetTarget()得到
            self.xm_Nav.userdata.pos_xm = Pose()
            # 这个target_mode == 1返回Pose(),如果== 0 返回名字
            self.xm_Nav.userdata.target_mode = 1
            # 这里的不可能产生aborted
            StateMachine.add('GETTAGET_POSE',
                             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'})
            # 如果找不到路径继续执行NavStack的execute(),知道找到为止
            StateMachine.add('NAV_GO',
                             NavStack(),
                             transitions={'succeeded': 'succeeded',
                                          'aborted': 'NAV_GO', 'error': 'error'},
                             remapping={'pos_xm': 'pos_xm'})

        self.xm_Find = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
                                    input_keys=['target', 'current_task'])
        with self.xm_Find:
            #            self.xm_Find.userdata.degree = 45.0
            self.xm_Find.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.xm_Find.userdata.pos_xm = Pose()
            StateMachine.add('GET_PEOPLE_POS',
                             FindPeople().find_people_,
                             transitions={
                                 'invalid': 'NAV_PEOPLE', 'valid': 'CLOSEKINECT', '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': 'CLOSEKINECT',
                                          'aborted': 'NAV_PEOPLE', 'error': 'error'},
                             remapping={'pos_xm': 'pos_xm'})

            # close the KinectV2
            StateMachine.add('CLOSEKINECT',
                             CloseKinect(),
                             transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
        self.pick_up = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
        with self.pick_up:
            self.pick_up.userdata.name = 'bag'
            self.pick_up.userdata.objmode = -1
            self.pick_up.userdata.arm_state = 'after_grasp_bag'
            self.pick_up.userdata.mode = 3
            self.pick_up.userdata.arm_ps = PointStamped()
            self.pick_up.userdata.arm_ps.header.frame_id = 'base_link'
            self.pick_up.userdata.arm_ps.point.x = 0.8
            self.pick_up.userdata.arm_ps.point.y = 0.0
            self.pick_up.userdata.arm_ps.point.z = 0.6
            StateMachine.add('RUNNODE',
                             RunOBJNode(),
                             transitions={'succeeded': 'GET_POSITION', 'aborted': 'aborted'})
            StateMachine.add('GET_POSITION',
                             FindObject(),
                             transitions={'succeeded': 'POS_JUSTFY', 'aborted': 'GET_POSITION', 'error': 'PICK_OBJ2'})
            self.pick_up.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_OBJ', 'aborted': 'FIND_AGAIN', 'error': 'PICK_OBJ2'},
                             remapping={'name': 'name', 'object_pos': 'object_pos', 'objmode': 'objmode'})
            StateMachine.add('PICK_OBJ',
                             ArmCmd(),
                             transitions={'succeeded': 'succeeded',
                                          'aborted': 'aborted', 'error': 'error'},
                             # 这里的aborted比赛时可以改成到READY_NAV
                             remapping={'arm_ps': 'object_pos'})
            StateMachine.add('PICK_OBJ2',
                             ArmCmd(),
                             transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error'})
            # self.pick_up.userdata.sentences = 'xiao meng can not find the thing'
            # StateMachine.add('SPEAK',
            #                     Speak(),
            #                     transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'})
            # StateMachine.add('READY_NAV',
            #                     ChangeArmState(),
            #                     transitions= {'succeeded':'CLOSEKINECT_OBJ','aborted':'aborted'})
            # StateMachine.add('CLOSEKINECT_OBJ',
            #                     Close_OBJ(),
            #                     transitions={'succeeded':'succeeded','aborted':'aborted'})

        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',
                             ArmCmd2(),
                             transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})

        self.help_me = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
        with self.help_me:
            # 这里可以预先定义一个值,识别错误后仍可以继续进行
            self.help_me.userdata.target = list()
            self.help_me.userdata.action = list()
            self.help_me.userdata.current_task = 0
            self.help_me.userdata.start_sentences = 'i will follow you'
            self.help_me.userdata.help_sentences = 'pleas help carry the groceries into the house, thank you'
            self.help_me.userdata.nav_sentences = 'i will go to the car, please follow me, my master'
            self.help_me.userdata.target_name = 'door'
            self.help_me.userdata.rec = 3.0
            self.help_me.userdata.go_point = Point(1, 0.0, 0.0)
            self.help_me.userdata.arrive_sentences = 'here is the car,my master'
            self.help_me.userdata.pos_car = gpsr_target['car']['pos']
            self.help_me.userdata.pos_door = gpsr_target['door']['pos']
            self.help_me.userdata.pos_out_door = gpsr_target['out_door']['pos']
            self.help_me.userdata.arm_mode_pick = 3
            self.help_me.userdata.arm_state_put = 'xm_place_bag'
            self.help_me.userdata.arm_ps = PointStamped(
                Header(0, 0, ''), Point(0, 0, 0))

            StateMachine.add('GET_START',  # 开始喽,获取语音信号,如果你说‘follow me’,那action就是follow
                             GetSignal(),
                             transitions={'succeeded': 'SPEAK_START', 'aborted': 'GET_START', 'error': 'error'})

            StateMachine.add('SPEAK_START',
                             Speak(),
                             transitions={'succeeded': 'FOLLOW',
                                          'aborted': 'FOLLOW', 'error': 'error'},
                             remapping={'sentences': 'start_sentences'})
            StateMachine.add('FOLLOW',
                             self.trace,
                             transitions={'succeeded': 'SET_POSITION', 'aborted': 'FOLLOW'})
            # StateMachine.add('CLOSEKINECT',
            #                     CloseKinect(),
            #                     transitions = {'succeeded':'PICK','aborted':'PICK'})

            #####not finished#####
            StateMachine.add('SET_POSITION',
                             SetPose(),
                             transitions={'succeeded': 'PICK', 'aborted': 'PICK'})
            StateMachine.add('PICK',
                             self.pick_up,
                             transitions={'succeeded': 'GET_TARGET',
                                          'aborted': 'GET_TARGET', 'error': 'error'},
                             remapping={'arm_mode': 'arm_mode_pick'})
            StateMachine.add('GET_TARGET',
                             GetSignal(),
                             transitions={'succeeded': 'NAV_ROOM', 'aborted': 'GET_TARGET', 'error': 'error'})
            StateMachine.add('NAV_OUT_DOOR',
                             NavStack(),
                             transitions={'succeeded': 'SIMPLE_MOVE1',
                                          'aborted': 'NAV_OUT_DOOR', 'error': 'error'},
                             remapping={'pos_xm': 'pos_out_door'})

            # 在开启导航之前首先让xm冲进房间
            # 使用userdata: go_point
            StateMachine.add('SIMPLE_MOVE1',
                             SimpleMove_move(),
                             remapping={'point': 'go_point'},
                             transitions={'succeeded': 'NAV_ROOM', 'aborted': 'NAV_ROOM', 'error': 'error'})

            StateMachine.add('NAV_ROOM',
                             self.xm_Nav,
                             transitions={'succeeded': 'PUT', 'aborted': 'aborted', 'error': 'error'})

            StateMachine.add('PUT',
                             self.sm_Place,
                             transitions={'succeeded': 'FIND',
                                          'aborted': 'FIND'},
                             remapping={'arm_state': 'arm_state_put'})
            StateMachine.add('FIND',
                             self.xm_Find,
                             transitions={'succeeded': 'ASK_HELP', 'aborted': 'aborted', 'error': 'error'})
            StateMachine.add('ASK_HELP',
                             Speak(),
                             transitions={'succeeded': 'SPEAK_NAV',
                                          'aborted': 'SPEAK_NAV', 'error': 'error'},
                             remapping={'sentences': 'help_sentences'})

            StateMachine.add('SPEAK_NAV',
                             Speak(),
                             transitions={'succeeded': 'SET_TARGET',
                                          'aborted': 'NAV_DOOR', 'error': 'error'},
                             remapping={'sentences': 'nav_sentences'})
            StateMachine.add('SET_TARGET',
                             SetTarget(),
                             transitions={
                                 'succeeded': 'NAV_DOOR', 'error': 'error'},
                             remapping={'pos_car': 'pos_car'})
            StateMachine.add('NAV_DOOR',
                             NavStack(),
                             transitions={'succeeded': 'DOORDETECT',
                                          'aborted': 'NAV_DOOR', 'error': 'error'},
                             remapping={'pos_xm': 'pos_door'})
            # 遇到问题是Doordetect是否要和导航同时运行
            # 将导航分为两部分,一个是导航到门,一个是导航到车
            StateMachine.add('DOORDETECT',
                             DoorDetect().door_detect_,
                             transitions={'invalid': 'WAIT', 'valid': 'DOORDETECT', 'preempted': 'aborted'})
            # 在刷新建图后等待一段时间
            # 使用userdata: rec
            StateMachine.add('WAIT',
                             Wait(),
                             transitions={
                                 'succeeded': 'SIMPLE_MOVE', 'error': 'error'},
                             remapping={'rec': 'rec'})

            # 在开启导航之前首先让xm冲进房间
            # 使用userdata: go_point
            StateMachine.add('SIMPLE_MOVE',
                             SimpleMove_move(),
                             remapping={'point': 'go_point'},
                             transitions={'succeeded': 'NAV_CAR', 'aborted': 'NAV_CAR', 'error': 'error'})
            #####not finished#####
            StateMachine.add('NAV_CAR',
                             self.xm_Nav,
                             transitions={'succeeded': 'SPEAK_ARRIVE',
                                          'aborted': 'NAV_CAR', 'error': 'error'},
                             remapping={'pos_xm': 'pos_car'})
            StateMachine.add('SPEAK_ARRIVE',
                             Speak(),
                             remapping={'sentences': 'arrive_sentences'},
                             transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error'})
        # self.navigation = Concurrence(outcomes=['succeeded','aborted'],
        #                               default_outcome = ['aborted'],
        #                               input_keys = ['car_position'],
        #                               outcome_cb = self.navigation_child_cb,
        #                               child_termination_cb = self.navigation_out_cb)
        # with self.navigation:
        #     Concurrence.add('NAV')
        #     Concurrence.add('DOORDETECT')
        # self.help_me = StateMachine(outcomes=['succeeded','aborted','error'])

        try:
            out = self.help_me.execute()
        except Exception, e:
            rospy.logerr('test failed , loser loser, don\'t have dinner')
            rospy.logerr(e)
Пример #44
0
    def __init__(self, target_object_key=None, target_frame='/map'):
        input_keys = ['in_room_name']
        if target_object_key is not None:
            input_keys.append(target_object_key)

        smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
                                    input_keys=input_keys,
                                    output_keys=['out_object_found', 'out_location_inside_room_name'])

        if target_object_key is None:
            target_object_key = 'in_target_object'
            self.userdata.in_target_object = ''  # No target object set! Before set to str(None), changed for SearchObjectWCSM
            going_to_pool = ["I'm going to look for objects.", "Maybe I find objects there. I'll have a look.",
                             "I think I know where objects can be."]
            object_not_found_pool = ["I can't see any object here.", "It seems that there aren't objects here.",
                                     "I haven't found any object here."]
        else:
            going_to_pool = ["I'm going to look if I find it.", "Maybe I find it there. I'll have a look.",
                             "I think I know where it can be.", "I think it can be there."]
            object_not_found_pool = ["I can't see it here.", "It seems that it isn't here.",
                                     "I haven't found it here."]

        object_found_pool = ["I can see the %s.", "Is that the %s? I think so!",
                             "The %s is just there.", "I found the %s.",
                             "Can't you see the %s? It's there!"]

        self.userdata.out_location_inside_room_name = 'tabletop'  # Always the same as it's not possible to know the name.
        self.userdata.objects_data = None  # To avoid errors the first time the check_if_remaining is called
        self.userdata.sensing_route = None  # Because at the very beginning there's no route
        self.userdata.tabletop_route = None  # Because at the very beginning there's no route
        self.userdata.out_object_found = None  # To avoid errors if there's an abort.

        with self:
            StateMachine.add('CHECK_IF_OBJECTS_REMAINING', CheckRemaining(),
                             remapping={'in_obj_list': 'objects_data'},
                             transitions={'empty': 'CHECK_IF_TABLE_ROUTE_EMPTY',
                                          'remaining': 'TELL_GO_RECOGNIZE'})

            @smach.cb_interface(input_keys=['in_route', 'in_table_poses', 'in_orientations'],
                                output_keys=['out_remaining_poses', 'out_remaining_orientations'],
                                outcomes=['remaining', 'empty'])
            def check_ttop_route_emptiness(userdata):
                if userdata.in_route:
                    # We get the remaining poses and orientations of the tabletop route
                    # and then we'll recalculate the route through that remaining nodes
                    remaining_poses = []
                    remaining_orientations = []
                    for index in userdata.in_route:  # Tabletop route has only the indices of the elements
                        remaining_poses.append(userdata.in_table_poses[index])
                        remaining_orientations.append(userdata.in_orientations[index])
                    userdata.out_remaining_poses = remaining_poses
                    userdata.out_remaining_orientations = remaining_orientations
                    return 'remaining'
                return 'empty'

            StateMachine.add('CHECK_IF_TABLE_ROUTE_EMPTY',
                             CBState(check_ttop_route_emptiness,
                                     input_keys=['in_route', 'in_table_poses', 'in_orientations'],
                                     output_keys=['out_remaining_poses', 'out_remaining_orientations'],
                                     outcomes=[succeeded, 'empty']),
                             remapping={'in_route': 'tabletop_route',
                                        'in_table_poses': 'tabletop_detect_poses',
                                        'in_orientations': 'tabletop_orientation_data',
                                        'out_remaining_poses': 'tabletop_detect_poses',
                                        'out_remaining_orientations': 'tabletop_orientation_data'},
                             transitions={'remaining': 'TABLETOPS_TSP_ROUTE',
                                          'empty': 'CHECK_IF_SENSING_ROUTE_EMPTY'})

            @smach.cb_interface(input_keys=['in_route'], output_keys=['out_remaining_nodes'],
                                outcomes=['remaining', 'empty'])
            def check_sensing_route_emptiness(userdata):
                if userdata.in_route:
                    # We get the remaining nodes of the route
                    # and then we'll recalculate the route through that remaining nodes
                    userdata.out_remaining_nodes = userdata.in_route  # The route itself has the nodes ordered.
                    return 'remaining'
                return 'empty'

            StateMachine.add('CHECK_IF_SENSING_ROUTE_EMPTY',
                             CBState(check_sensing_route_emptiness,
                                     input_keys=['in_route'],
                                     output_keys=['out_remaining_nodes'],
                                     outcomes=[succeeded, 'empty']),
                             remapping={'in_route': 'sensing_route',
                                        'out_remaining_nodes': 'sens_locations'},
                             transitions={'remaining': 'SENSING_LOCATIONS_TSP_ROUTE',
                                          'empty': 'CALCULATE_TTOP_SENSING_LOCATIONS'})

            StateMachine.add('CALCULATE_TTOP_SENSING_LOCATIONS', CalculateSensingLocations(),
                             remapping={'in_room_name': 'in_room_name', 'out_guards': 'sens_locations'},
                             transitions={succeeded: 'SENSING_LOCATIONS_TSP_ROUTE', 'no_corner_info': aborted})

            StateMachine.add('SENSING_LOCATIONS_TSP_ROUTE', TSPState(HC=HC),
                             remapping={'in_nodes': 'sens_locations', 'out_route': 'sensing_route'},
                             transitions={succeeded: 'GET_NEXT_SENSING_LOCATION'})

            @smach.cb_interface(input_keys=['in_sensing_route'], output_keys=['out_sensing_route', 'out_next_sensing_pose'],
                                outcomes=[succeeded, 'empty_route'])
            def get_next_loc_from_route(userdata):
                # Returns the next sensing location to go and removes it from the route
                if not userdata.in_sensing_route:  # The sensing route is empty...
                    return 'empty_route'
                next_pos = userdata.in_sensing_route[0]
                userdata.out_sensing_route = userdata.in_sensing_route[1:]
                pose = Pose()
                pose.position.x = next_pos[0]
                pose.position.y = next_pos[1]
                pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.57))  # FIXME is it possible to avoid a fixed orientation?
                userdata.out_next_sensing_pose = pose
                return succeeded

            StateMachine.add('GET_NEXT_SENSING_LOCATION',
                             CBState(get_next_loc_from_route,
                                     input_keys=['in_sensing_route'],
                                     output_keys=['out_sensing_route', 'out_next_sensing_pose'],
                                     outcomes=[succeeded, 'empty_route']),
                             remapping={'out_sensing_route': 'sensing_route',
                                        'in_sensing_route': 'sensing_route',
                                        'out_next_sensing_pose': 'next_sensing_pose'},
                             transitions={succeeded: 'MOVE_TO_SENSING_LOCATION',
                                          'empty_route': 'CALCULATE_TTOP_SENSING_LOCATIONS'})

            StateMachine.add('MOVE_TO_SENSING_LOCATION',
                             MoveActionState("/map", goal_key='next_sensing_pose'),
                             transitions={succeeded: 'DETECT_TABLETOPS_AND_ANNOUNCE',
                                          aborted: 'GET_NEXT_SENSING_LOCATION'})  # FIXME if aborted we get the next pose.

            tell_and_search = Concurrence(outcomes=[succeeded, aborted, preempted, 'no_tables'],
                                          default_outcome=aborted,
                                          output_keys=['tabletop_detect_poses', 'tabletop_orientation_data'],
                                          outcome_map={succeeded: {'ANNOUNCE_SEARCHING': succeeded,
                                                                   'DETECT_TABLETOPS_ZONE': succeeded},
                                                       'no_tables': {'DETECT_TABLETOPS_ZONE': 'no_tables'}})
            with tell_and_search:
                searching_tables_pool = ["I'm looking for places that can contain objects.",
                                         "I'm searching a place that can have objects.",
                                         "I'm searching the best place to find objects."]
                Concurrence.add('ANNOUNCE_SEARCHING',
                                SpeakActionFromPoolStateMachine(searching_tables_pool))

                Concurrence.add('DETECT_TABLETOPS_ZONE',
                                DetectTablesOfZoneSM(distance_treshold=DIST_BETWEEN_TABLES, dist_to_table=DIST_TO_TABLE),
                                remapping={'out_table_pose_list': 'tabletop_detect_poses',
                                           'out_table_orientation_list': 'tabletop_orientation_data'})

            StateMachine.add('DETECT_TABLETOPS_AND_ANNOUNCE', tell_and_search,
                             transitions={succeeded: 'TABLETOPS_TSP_ROUTE', 'no_tables': 'GET_NEXT_SENSING_LOCATION'},
                             remapping={'tabletop_orientation_data': 'tabletop_orientation_data',
                                        'tabletop_detect_poses': 'tabletop_detect_poses'})

            # @smach.cb_interface(input_keys=['in_tabletop_detect_data'], output_keys=['out_table_node_list'],
            #                     outcomes=[succeeded])
            # def check_ttop_data(userdata):
            #     #FIXME -> May be necessary to filter in some way the already visited tables...
            #     #tabletop_detect_data is a list of tuples (pos, orientation), so we get only the poses for the TSP
            #     userdata.out_table_node_list = reduce(lambda acc, x: acc + [x[0]], userdata.in_tabletop_detect_data, [])

            #     return succeeded

            # StateMachine.add('CHECK_TABLETOP_DATA',
            #                  CBState(check_ttop_data, input_keys=['in_tabletop_detect_data'],
            #                          output_keys=['out_object_detection_pose'],
            #                          outcomes=[succeeded]),
            #                  remapping={'in_tabletop_detect_data': 'tabletop_detect_data',
            #                             'out_table_node_list': 'table_node_list'},
            #                  transitions={succeeded: 'TELL_GO_RECOGNIZE'})

            StateMachine.add('TABLETOPS_TSP_ROUTE', TSPState(HC=HC, indices=True),
                             remapping={'in_nodes': 'tabletop_detect_poses',
                                        'out_route': 'tabletop_route'},
                             transitions={succeeded: 'GET_NEXT_TABLETOP_LOCATION'})

            @smach.cb_interface(input_keys=['in_table_route', 'in_orient_list', 'in_pose_list'],
                                output_keys=['out_table_route', 'out_object_detection_pose'],
                                outcomes=[succeeded, 'empty_route'])
            def get_next_loc_from_table_route(userdata):
                # Returns the next tabletop location to go and removes it from the route
                if not userdata.in_table_route:  # The route is empty, so we recalculate the sensing route and go again
                    return 'empty_route'

                node_index = userdata.in_table_route[0]
                print('node_index: %d, length in_table_route: %d' % (node_index, len(userdata.in_table_route)))
                next_pos = userdata.in_pose_list[node_index]

                # Only the index is removed from the list, to preserve the index order of the other elements
                userdata.out_table_route = userdata.in_table_route[1:]
                pose = Pose()
                pose.position.x = next_pos[0]
                pose.position.y = next_pos[1]
                pose.orientation = userdata.in_orient_list[node_index]
                userdata.out_object_detection_pose = pose
                return succeeded

            StateMachine.add('GET_NEXT_TABLETOP_LOCATION',
                             CBState(get_next_loc_from_table_route,
                                     input_keys=['in_table_route', 'in_orient_list', 'in_pose_list'],
                                     output_keys=['out_table_route', 'out_object_detection_pose'],
                                     outcomes=[succeeded, 'empty_route']),
                             remapping={'out_table_route': 'tabletop_route',
                                        'in_table_route': 'tabletop_route',
                                        'in_orient_list': 'tabletop_orientation_data',
                                        'in_pose_list': 'tabletop_detect_poses',
                                        'out_object_detection_pose': 'object_detection_pose'},
                             transitions={succeeded: 'TELL_GO_RECOGNIZE',
                                          'empty_route': 'SENSING_LOCATIONS_TSP_ROUTE'})

            StateMachine.add('TELL_GO_RECOGNIZE', TellGoRecognizeSM(msg_pool=going_to_pool, arg_key=None),
                             remapping={'out_objects_data': 'objects_data',
                                        'in_target_object': target_object_key,
                                        'in_location_pose_in_map': 'object_detection_pose'},
                             transitions={'move_failed': 'GET_NEXT_SENSING_LOCATION',
                                          'no_object_found': 'CHECK_IF_OBJECT_FOUND',
                                          succeeded: 'CHECK_IF_OBJECT_FOUND',
                                          aborted: aborted})  # FIXME aborted should abort everything?

            StateMachine.add('CHECK_IF_OBJECT_FOUND',
                             CheckObjectAndRemoveFromList(target_frame),
                             remapping={'out_object_found': 'out_object_found',
                                        'in_objects_data': 'objects_data',
                                        'out_objects_data': 'objects_data',
                                        'in_target_object': target_object_key},
                             transitions={succeeded: 'PREPARE_POOL_ARGS', aborted: 'TELL_NO_OBJECT_FOUND'})

            StateMachine.add('TELL_NO_OBJECT_FOUND',
                             SpeakActionFromPoolStateMachine(object_not_found_pool, arg_key=None),
                             remapping={},
                             transitions={succeeded: 'GET_NEXT_SENSING_LOCATION', aborted: 'GET_NEXT_SENSING_LOCATION'})

            @smach.cb_interface(input_keys=['in_first_object', 'in_location_name'], output_keys=['out_tell_arg'],
                                outcomes=[succeeded])
            def prepare_userdata(userdata):
                userdata.out_tell_arg = userdata.in_first_object.name
                return succeeded

            StateMachine.add('PREPARE_POOL_ARGS',
                             CBState(prepare_userdata,
                                     input_keys=['in_first_object', 'in_location_name'],
                                     output_keys=['out_tell_arg'], outcomes=[succeeded]),
                             remapping={'in_first_object': 'out_object_found',
                                        'out_tell_arg': 'tell_arg'},
                             transitions={succeeded: 'TELL_OBJECT_RECOGNIZED'})

            StateMachine.add('TELL_OBJECT_RECOGNIZED',
                             SpeakActionFromPoolStateMachine(object_found_pool, arg_key="tell_arg"),
                             remapping={'tell_arg': 'tell_arg'},
                             transitions={succeeded: succeeded, aborted: aborted})
Пример #45
0
    def __init__(self):

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

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

        # How long do we have to get to each waypoint?
        self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds
        
        # Initialize the patrol counter
        self.patrol_count = 0
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")

        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
        
        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))    
        
        rospy.loginfo("Connected to move_base action server")

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

        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        # -0.163200, 0.044660, 0.193186
        self.waypoints.append(Pose(Point(0.9579,    1.8710, 0.0), quaternions[3]))
        self.waypoints.append(Pose(Point(0.7555,    0.0692, 0.0), quaternions[1]))
        self.waypoints.append(Pose(Point(-0.72511,  0.4952, 0.0), quaternions[0]))
        self.waypoints.append(Pose(Point(0.167730,  2.18168, 0.0), quaternions[2]))

        position_locations = list()
        position_locations.append(('position1', self.waypoints[0]))
        position_locations.append(('position2', self.waypoints[1]))
        position_locations.append(('position3', self.waypoints[2]))
        position_locations.append(('position4', self.waypoints[3]))

        self.position_locations = OrderedDict(position_locations)
        
        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
        
        rospy.loginfo("Starting Tasks")
        
        # Initialize a number of parameters and variables
        # setup_task_environment(self)
        RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints)

        ''' Create individual state machines for assigning tasks to each position '''

        # Initialize the overall state machine
        self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted'])
            
        # Build the find treasure state machine from the nav states and treasure finding states
        with self.sm_find_treasure:
            # First State Machine
            sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted'])
            sm_nav.userdata.waypoints = self.waypoints
            sm_nav.userdata.waypoints_primary_count = len(self.waypoints)
            rospy.loginfo(sm_nav.userdata.waypoints)

            with sm_nav:
                # StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
                #              transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''},
                #              remapping={'waypoint_out':'patrol_waypoint',
                #                         'waypoints_primary_count':'waypoints_primary_count'})
            
                StateMachine.add('NAV_WAYPOINT', NavState(),
                             transitions={'succeeded':'NAV_WAYPOINT', 
                                          'aborted':'', 
                                          'preempted':''},
                             remapping={'waypoint_in':'patrol_waypoint',
                                        'waypoints_primary_count':'waypoints_primary_count'})

            # Second State Machine
            sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted'])
            sm_read_tags.userdata.waypoints = self.waypoints

            with sm_read_tags:
                StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers), 
                    transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''})

            # Third State Machine
            sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted'])

            with sm_detect_faces:
                StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image),
                    transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''})

            # Forth State Machine
            sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted'])

            with sm_recognize_faces:
                StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''})
                # goal = FaceRecognitionGoal()
                # goal.order_id = 1
                # goal.order_argument = ''
                # StateMachine.add('recognize_face', SimpleActionState('face_recognition',
                #                 FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''})

            sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded',
            child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb)

            with sm_con:
                Concurrence.add('SM_NAV', sm_nav)
                Concurrence.add('SM_READ_TAGS', sm_read_tags)
                # Concurrence.add('SM_DETECT_FACES', sm_detect_faces)
                Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces)

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

            with sm_estimate_position:
                StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers),
                    transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''})

            StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'})
            StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''})
                        
        # Create and start the SMACH introspection server sm_find_treasure
        intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = self.sm_find_treasure.execute()

        rospy.loginfo('the length now is')
        rospy.loginfo(self.waypoints)

        intro_server.stop()
    def __init__(self):
        rospy.init_node('random_patrol', anonymous=False)
        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # Initialize a number of parameters and variables
        #setup_task_environment(self)
        

        # 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




        # 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','aborted':'NAV_WAYPOINT'},
                             remapping={'waypoint_out':'patrol_waypoint'})
            
            StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
                             transitions={'succeeded':'', 
                                          'aborted':''},
                             remapping={'waypoint_in':'patrol_waypoint'})
            
	
	# Create the nav_patrol state machine using a Concurrence container
        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_nav machine and a battery MonitorState to the nav_patrol machine             
        with self.sm_concurrent:
           Concurrence.add('SM_NAV', self.sm_patrol)
           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()
Пример #47
0
def main():
    rospy.init_node('smach_usecase_step_05')

    # 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':'TELEPORT2'})

        # 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_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
            Concurrence.add('SMALL',
                    SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
                        goal = polygon_small))


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

    # Set preempt handler
    smach.set_preempt_handler(sm0)

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

    # Signal handler
    rospy.spin()
Пример #48
0
    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()
Пример #49
0
    def __init__(self):
        rospy.init_node('explain_history_concurrence', anonymous=False)

        # 设置关闭机器人函数(stop the robot)
        rospy.on_shutdown(self.shutdown)

        # 初始化一些参数和变量
        setup_task_environment(self)

        # 跟踪到达目标位置的成功率
        self.n_succeeded = 0
        self.n_aborted = 0
        self.n_preempted = 0

        # 保存上一个或者当前的导航目标点的变量
        self.last_nav_state = None

        # 指示是否正在充电的标志
        self.recharging = False

        # 保存导航目标点的列表
        nav_states = {}

        # 把waypoints变成状态机的状态
        for waypoint in self.room_locations.iterkeys():
            nav_goal = MoveBaseGoal()
            nav_goal.target_pose.header.frame_id = 'map'
            nav_goal.target_pose.pose = self.room_locations[waypoint]
            move_base_state = SimpleActionState(
                'move_base',
                MoveBaseAction,
                goal=nav_goal,
                result_cb=self.move_base_result_cb,
                exec_timeout=rospy.Duration(10.0),
                server_wait_timeout=rospy.Duration(10.0))
            # nav_states.append(move_base_state)
            nav_states[waypoint] = move_base_state

        # 为扩展底座(docking station)创建一个MoveBaseAction state
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'map'
        nav_goal.target_pose.pose = self.docking_station_pose
        nav_docking_station = SimpleActionState(
            'move_base',
            MoveBaseAction,
            goal=nav_goal,
            result_cb=self.move_base_result_cb,
            exec_timeout=rospy.Duration(20.0),
            server_wait_timeout=rospy.Duration(10.0))

        # 为written words子任务创建一个状态机
        sm_written_words = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])
        # 然后添加子任务
        with sm_written_words:
            StateMachine.add('EXPLAIN_HISTORY',
                             WrittenWords('written_words', 5),
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

        # 为rule子任务创建一个状态机
        sm_rule = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        # 然后添加子任务
        with sm_rule:
            StateMachine.add('EXPLAIN_HISTORY',
                             Rule('rule', 5),
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

        # 为life子任务创建一个状态机
        sm_life = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        # 然后添加子任务
        with sm_life:
            StateMachine.add('EXPLAIN_HISTORY',
                             Life('life', 5),
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

        # 为faith子任务创建一个状态机
        sm_faith = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        # 然后添加子任务
        with sm_faith:
            StateMachine.add('EXPLAIN_HISTORY',
                             Faith('faith', 5),
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

        # 初始化导航的状态机
        self.sm_nav = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        # 使用transitions将导航的状态添加到状态机
        with self.sm_nav:
            StateMachine.add('START',
                             nav_states['explanatory_text'],
                             transitions={
                                 'succeeded': 'WRITTEN_WORDS',
                                 'aborted': 'WRITTEN_WORDS',
                                 'preempted': 'WRITTEN_WORDS'
                             })
            ''' Add the living room subtask(s) '''
            StateMachine.add('WRITTEN_WORDS',
                             nav_states['explanatory_text'],
                             transitions={
                                 'succeeded': 'WRITTEN_WORDS_TASKS',
                                 'aborted': 'RULE',
                                 'preempted': 'RULE'
                             })

            # 当任务完成时, 继续进行kitchen任务
            StateMachine.add('WRITTEN_WORDS_TASKS',
                             sm_written_words,
                             transitions={
                                 'succeeded': 'RULE',
                                 'aborted': 'RULE',
                                 'preempted': 'RULE'
                             })
            ''' Add the kitchen subtask(s) '''
            StateMachine.add('RULE',
                             nav_states['explain_the_rule'],
                             transitions={
                                 'succeeded': 'RULE_TASKS',
                                 'aborted': 'LIFE',
                                 'preempted': 'LIFE'
                             })

            # 当任务完成时, 继续进行bathroom任务
            StateMachine.add('RULE_TASKS',
                             sm_rule,
                             transitions={
                                 'succeeded': 'LIFE',
                                 'aborted': 'LIFE',
                                 'preempted': 'LIFE'
                             })
            ''' Add the bathroom subtask(s) '''
            StateMachine.add('LIFE',
                             nav_states['explain_life'],
                             transitions={
                                 'succeeded': 'LIFE_TASKS',
                                 'aborted': 'FAITH',
                                 'preempted': 'FAITH'
                             })

            # 当任务完成时, 继续进行hallway任务
            StateMachine.add('LIFE_TASKS',
                             sm_life,
                             transitions={
                                 'succeeded': 'FAITH',
                                 'aborted': 'FAITH',
                                 'preempted': 'FAITH'
                             })
            ''' Add the hallway subtask(s) '''
            StateMachine.add('FAITH',
                             nav_states['explain_faith'],
                             transitions={
                                 'succeeded': 'FAITH_TASKS',
                                 'aborted': '',
                                 'preempted': ''
                             })

            # 当任务完成时, stop
            StateMachine.add('FAITH_TASKS',
                             sm_faith,
                             transitions={
                                 'succeeded': '',
                                 'aborted': '',
                                 'preempted': ''
                             })

        # 在sm_nav状态机中注册一个回调函数以启动状态转换(state transitions)
        self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])

        # 初始化充电的状态机
        self.sm_recharge = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_recharge:
            StateMachine.add('NAV_DOCKING_STATION',
                             nav_docking_station,
                             transitions={'succeeded': 'RECHARGE_BATTERY'})
            StateMachine.add('RECHARGE_BATTERY',
                             ServiceState(
                                 'battery_simulator/set_battery_level',
                                 SetBatteryLevel,
                                 100,
                                 response_cb=self.recharge_cb),
                             transitions={'succeeded': ''})

        # 使用并发容器(Concurrence container)创建nav_patrol状态机
        self.nav_patrol = Concurrence(
            outcomes=['succeeded', 'recharge', 'stop'],
            default_outcome='succeeded',
            child_termination_cb=self.concurrence_child_termination_cb,
            outcome_cb=self.concurrence_outcome_cb)

        # 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面
        with self.nav_patrol:
            Concurrence.add('SM_NAV', self.sm_nav)
            Concurrence.add(
                'MONITOR_BATTERY',
                MonitorState("battery_level", Float32, self.battery_cb))

        # 创建顶层状态机
        self.sm_top = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        # 将nav_patrol,sm_recharge和Stop添加到sm_top状态机
        with self.sm_top:
            StateMachine.add('PATROL',
                             self.nav_patrol,
                             transitions={
                                 'succeeded': 'PATROL',
                                 'recharge': 'RECHARGE',
                                 'stop': 'STOP'
                             })
            StateMachine.add('RECHARGE',
                             self.sm_recharge,
                             transitions={'succeeded': 'PATROL'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})

        # 创建并开始SMACH introspection server
        intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
        intro_server.start()

        # 运行状态机
        sm_outcome = self.sm_top.execute()

        rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))

        intro_server.stop()
class SMACHAI():
    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()


    def empty_cb(self, userdata, msg):
        return False

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

    def start_cb(self, userdata, msg):
	rospy.loginfo("Start !")
        return False

    def color_cb(self, userdata, msg):
        rospy.loginfo("Color " + str(msg.data))
	self.robot_side = msg.data

	self.sm_action1.userdata.robot_side = self.robot_side
	self.sm_action2.userdata.robot_side = self.robot_side
	self.sm_action3.userdata.robot_side = self.robot_side
	self.sm_action4.userdata.robot_side = self.robot_side
	self.sm_action5.userdata.robot_side = self.robot_side
	self.sm_action6.userdata.robot_side = self.robot_side
	self.sm_action7.userdata.robot_side = self.robot_side
	
	self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE

        return False

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


    # 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_JO'] == 'succeeded':
            return True
        if outcome_map['SM_CAROLE'] == 'succeeded':
            return True
        if outcome_map['SM_EAT'] == 'succeeded':
            return True
        if outcome_map['SM_SHOWER'] == 'succeeded':
            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['SM_JO'] == 'succeeded':
            return 'succeeded'
        elif outcome_map['SM_CAROLE'] == 'succeeded':
            return 'succeeded'
        # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop'
        elif outcome_map['SM_EAT'] == 'succeeded':
            return 'succeeded'
        elif outcome_map['SM_SHOWER'] == 'succeeded':
            return 'succeeded'
        else:
            return 'stop'


    # Gets called when ANY child state terminates
    def jo_awake_child_termination_cb(self, outcome_map):
        # If the current navigation task has succeeded, return True
        if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded':
            return True
        if outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded':
            return True
        if outcome_map['SM_GO_OUT'] == 'succeeded':
            return True
        else:
            return False


    # Gets called when ALL child states are terminated
    def jo_awake_outcome_cb(self, outcome_map):
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['SM_GO_TO_SLEEP'] == 'succeeded':
            return 'go_sleep'
        elif outcome_map['SM_BOTH_GO_TO_SLEEP'] == 'succeeded':
            return 'go_sleep'
        elif outcome_map['SM_GO_OUT'] == 'succeeded':
            return 'go_out'
        else:
            return 'stop'


    # Gets called when ANY child state terminates
    def jo_sleep_child_termination_cb(self, outcome_map):
        # If the current navigation task has succeeded, return True
        if outcome_map['SM_WAKE_UP'] == 'succeeded':
            return True
        if outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded':
            return True
        else:
            return False


    # Gets called when ALL child states are terminated
    def jo_sleep_outcome_cb(self, outcome_map):
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['SM_WAKE_UP'] == 'succeeded':
            return 'wake_up'
        elif outcome_map['SM_BOTH_WAKE_UP'] == 'succeeded':
            return 'wake_up'
        else:
            return 'stop'

    # Gets called when ANY child state terminates
    def jo_out_child_termination_cb(self, outcome_map):
        # If the current navigation task has succeeded, return True
        if outcome_map['SM_BACK_HOME'] == 'succeeded':
            return True
        if outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded':
            return True
        else:
            return False


    # Gets called when ALL child states are terminated
    def jo_out_outcome_cb(self, outcome_map):
        # If the battery is below threshold, return the 'recharge' outcome
        if outcome_map['SM_BACK_HOME'] == 'succeeded':
            return 'back_home'
        elif outcome_map['SM_BOTH_BACK_HOME'] == 'succeeded':
            return 'back_home'
        else:
            return 'stop'







    def shutdown(self):
        rospy.loginfo("Stopping the home automation...")
        
        self.sm_top.request_preempt()
        
        
        rospy.sleep(1)
Пример #51
0
    def __init__(self):
        rospy.init_node('deliver_food', anonymous=False)
        self.initialize_destination()
        self.lalala = 100

        # Track success rate of getting to the goal locations
        self.n_succeeded = 0
        self.n_aborted = 0
        self.n_preempted = 0

        nav_states = {}
        
        for room in self.room_locations.iterkeys():
            nav_goal = MoveBaseGoal()
            nav_goal.target_pose.header.frame_id = 'map'
            nav_goal.target_pose.pose = self.room_locations[room]
            move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, 
                                                exec_timeout=rospy.Duration(60.0),
                                                server_wait_timeout=rospy.Duration(10.0))
            nav_states[room] = move_base_state
            rospy.loginfo(room + " -> [" + str(round(self.room_locations[room].position.x,2)) + ", " + str(round(self.room_locations[room].position.y,2)) + "]" )

        sm_rotate_search = Concurrence(outcomes=['find', 'not_find'],
                                        default_outcome='not_find',
                                        child_termination_cb=self.concurrence_child_termination_callback,
                                        outcome_cb=self.concurrence_outcome_callback)

        with sm_rotate_search:
            Concurrence.add('ROTATE', Rotate360() )
            Concurrence.add('SEARCH', SearchTable() )        

        sm_table1 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table1:
            StateMachine.add('GOTO_TABLE1', nav_states['table1'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN',
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        sm_table2 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table2:
            StateMachine.add('GOTO_TABLE2', nav_states['table2'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN',
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        sm_table3 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table3:
            StateMachine.add('GOTO_TABLE3', nav_states['table3'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN',
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        sm_table4 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table4:
            StateMachine.add('GOTO_TABLE4', nav_states['table4'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN',
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        sm_table5 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table5:
            StateMachine.add('GOTO_TABLE5', nav_states['table5'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN', 
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        sm_table6 = StateMachine(outcomes=['succeeded','aborted','preempted'])
        with sm_table6:
            StateMachine.add('GOTO_TABLE6', nav_states['table6'], transitions={'succeeded':'ROTATE_SEARCH',
                                                                               'aborted':'GOTO_KITCHEN',
                                                                               'preempted':'GOTO_KITCHEN'})
            StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
                                                                             'not_find':'GOTO_KITCHEN'})
            StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
                                                                              'aborted':'GOTO_KITCHEN',
                                                                              'preempted':'GOTO_KITCHEN'})
            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
                                                                                 'aborted':'',
                                                                                 'preempted':''})

        # let's initialize the overall state machine
        sm_deliverfood = StateMachine(outcomes=['succeeded','aborted','preempted'])

        with sm_deliverfood:
            StateMachine.add('COMPUTER_VISION_TASK', ComputerVision(), transitions={'detect1':'TABLE_ONE_TASK',
                                                                                  'detect2':'TABLE_TWO_TASK',
                                                                                  'detect3':'TABLE_THREE_TASK',
                                                                                  'detect4':'TABLE_FOUR_TASK',
                                                                                  'detect5':'TABLE_FIVE_TASK',
                                                                                  'detect6':'TABLE_SIX_TASK'})
            StateMachine.add('TABLE_ONE_TASK',sm_table1, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
            StateMachine.add('TABLE_TWO_TASK',sm_table2, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
            StateMachine.add('TABLE_THREE_TASK',sm_table3, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
            StateMachine.add('TABLE_FOUR_TASK',sm_table4, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
            StateMachine.add('TABLE_FIVE_TASK',sm_table5, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
            StateMachine.add('TABLE_SIX_TASK',sm_table6, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})

            StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})

        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('deliver_food', sm_deliverfood, '/SM_ROOT')
        intro_server.start()
        
        # Execute the state machine
        sm_outcome = sm_deliverfood.execute()
        rospy.on_shutdown(self.shutdown)
    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()
Пример #53
0
def main():
    rospy.init_node('smach_usecase_step_06')

    # 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.set_preempt_handler(sm0)

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

    # Signal handler
    rospy.spin()
    def __init__(self):
        rospy.init_node('patrol_smach_concurrence', 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 variable to hold the last/current navigation goal
        self.last_nav_state = None
        
        # A flag to indicate whether or not we are rechargin
        self.recharging = False
        
        # A list to hold then navigation goa
        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)
            
            
        # Create a MoveBaseAction state for the docking station
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'map'
        nav_goal.target_pose.pose = self.docking_station_pose
        nav_docking_station = 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))

        # Initialize the navigation state machine
        self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        
        # Add the nav states to the state machine with the appropriate transitions
        with self.sm_nav:
            StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
            StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
            StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
            StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
            StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''})
        
        # Register a callback function to fire on state transitions within the sm_nav state machine
        self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])

        # Initialize the recharge state machine
        self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
        
        with self.sm_recharge:
            StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':''})
            #StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), 
                             #transitions={'succeeded':''})        

        # Create the nav_patrol state machine using a Concurrence container
        self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.concurrence_child_termination_cb,
                                        outcome_cb=self.concurrence_outcome_cb)
        
        # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine             
        with self.nav_patrol:
           Concurrence.add('SM_NAV', self.sm_nav)
           Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, 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('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', '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()
Пример #55
0
    def __init__(self):
        rospy.init_node('patrol_smach_concurrence', 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 variable to hold the last/current navigation goal
        self.last_nav_state = None

        # A flag to indicate whether or not we are rechargin
        self.recharging = False

        # A list to hold then navigation goa
        nav_states = list()

        location_goal = MoveBaseGoal()
        result_goal = MoveBaseGoal()

        #recognize_goal = VisionGoal()
        #flag = 1

        class wait(State):
            def __init__(self):
                State.__init__(self, outcomes=['succeeded', 'aborted'])

            def execute(self, userdata):
                if flag == 0:
                    rospy.loginfo('waitting')
                    return 'aborted'
                else:
                    return 'succeeded'

        class MoveItDemo(State):

            # spin() simply keeps python from exiting until this node is stopped

            def __init__(self):
                # Initialize the move_group API
                State.__init__(self, outcomes=['succeeded', 'aborted'])

            def execute(self, userdata):
                moveit_commander.roscpp_initialize(sys.argv)

                #rospy.init_node('moveit_ik')
                #rospy.Subscriber("chatter", Pose, callback)
                # Initialize the move group for the right arm
                right_arm = moveit_commander.MoveGroupCommander('right_arm')

                # Get the name of the end-effector link
                end_effector_link = right_arm.get_end_effector_link()

                # Set the reference frame for pose targets
                reference_frame = 'base_footprint'

                # Set the right arm reference frame accordingly
                right_arm.set_pose_reference_frame(reference_frame)

                # Allow replanning to increase the odds of a solution
                right_arm.allow_replanning(True)

                # Allow some leeway in position (meters) and orientation (radians)
                right_arm.set_goal_position_tolerance(0.11)
                right_arm.set_goal_orientation_tolerance(0.15)

                # Start the arm in the "resting" pose stored in the SRDF file
                right_arm.set_named_target('right_arm_init')
                right_arm.go()
                rospy.sleep(2)

                # Set the target pose.  This particular pose has the gripper oriented horizontally
                # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
                # the center of the robot base.
                target_pose = PoseStamped()
                target_pose.header.frame_id = reference_frame
                target_pose.header.stamp = rospy.Time.now()
                #global a,b,c,d,e,f,g
                target_pose.pose.position.x = 0.3
                target_pose.pose.position.y = -0.45
                target_pose.pose.position.z = 1.35
                target_pose.pose.orientation.x = 0
                target_pose.pose.orientation.y = 0
                target_pose.pose.orientation.z = 0
                target_pose.pose.orientation.w = 1
                #Set the start state to the current state
                right_arm.set_start_state_to_current_state()
                #print a
                # Set the goal pose of the end effector to the stored pose
                right_arm.set_pose_target(target_pose, end_effector_link)

                # Plan the trajectory to the goal
                traj = right_arm.plan()

                # Execute the planned trajectory
                right_arm.execute(traj)

                # Pause for a second
                rospy.sleep(5)
                #right_arm.set_named_target('right_arm_init')
                right_arm.go()
                return 'succeeded'

        class MoveItDemo1(State):

            # spin() simply keeps python from exiting until this node is stopped

            def __init__(self):
                # Initialize the move_group API
                State.__init__(self, outcomes=['succeeded', 'aborted'])

            def execute(self, userdata):
                moveit_commander.roscpp_initialize(sys.argv)

                #rospy.init_node('moveit_ik')
                #rospy.Subscriber("chatter", Pose, callback)
                # Initialize the move group for the right arm
                right_arm = moveit_commander.MoveGroupCommander('right_arm')

                # Get the name of the end-effector link
                end_effector_link = right_arm.get_end_effector_link()

                # Set the reference frame for pose targets
                reference_frame = 'base_footprint'

                # Set the right arm reference frame accordingly
                right_arm.set_pose_reference_frame(reference_frame)

                # Allow replanning to increase the odds of a solution
                right_arm.allow_replanning(True)

                right_arm.set_named_target('right_arm_init')
                right_arm.go()
                return 'succeeded'

        # 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(20.0),
                server_wait_timeout=rospy.Duration(20.0))
            nav_states.append(move_base_state)

        # Create a MoveBaseAction state for the docking station
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'map'
        nav_goal.target_pose.pose = self.docking_station_pose
        nav_docking_station = SimpleActionState(
            'move_base',
            MoveBaseAction,
            goal=nav_goal,
            result_cb=self.move_base_result_cb,
            exec_timeout=rospy.Duration(20.0),
            server_wait_timeout=rospy.Duration(10.0))
        pose_target = geometry_msgs.msg.Pose()

        pose_target.orientation.w = 0.1
        pose_target.position.x = 0.7
        pose_target.position.y = -0.0
        pose_target.position.z = 1.1

        result_goal.target_pose.header.frame_id = 'map'
        result_goal.target_pose.pose = (Pose(Point(0.5, 1.5, 0.0),
                                             Quaternion(0.0, 0.0, 0.0, 1.0)))

        # Initialize the navigation state machine
        self.sm_nav = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        # Add the nav states to the state machine with the appropriate transitions
        with self.sm_nav:
            StateMachine.add('WAITTING',
                             wait(),
                             transitions={
                                 'succeeded': 'NAV_STATE_0',
                                 'aborted': 'WAITTING'
                             })
            StateMachine.add('NAV_STATE_0',
                             nav_states[0],
                             transitions={
                                 'succeeded': 'NAV_STATE_1',
                                 'aborted': 'NAV_STATE_1'
                             })
            StateMachine.add('NAV_STATE_1',
                             SimpleActionState('move_base',
                                               MoveBaseAction,
                                               goal=location_goal),
                             transitions={
                                 'succeeded': 'ARM',
                                 'aborted': 'ARM'
                             })

            #StateMachine.add('VISION', SimpleActionState('drv_action', VisionAction, goal=recognize_goal),
            #transitions={'succeeded': 'ARM', 'aborted': 'ARM'})

            StateMachine.add('ARM',
                             MoveItDemo(),
                             transitions={
                                 'succeeded': 'NAV_STATE_2',
                                 'aborted': 'NAV_STATE_2'
                             })
            StateMachine.add('NAV_STATE_2',
                             nav_states[0],
                             transitions={
                                 'succeeded': 'ARM1',
                                 'aborted': 'ARM1'
                             })
            StateMachine.add('ARM1',
                             MoveItDemo1(),
                             transitions={
                                 'succeeded': '',
                                 'aborted': ''
                             })

            # StateMachine.add('NAV_STATE_2', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']),
            #                  transitions={'succeeded': 'NAV_STATE_0'}, remapping={'target_pose': 'user_data'})

        # Register a callback function to fire on state transitions within the sm_nav state machine
        self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])

        # Initialize the recharge state machine
        self.sm_recharge = StateMachine(
            outcomes=['succeeded', 'aborted', 'preempted'])

        with self.sm_recharge:
            StateMachine.add('NAV_DOCKING_STATION',
                             nav_docking_station,
                             transitions={'succeeded': 'RECHARGE_BATTERY'})
            StateMachine.add('RECHARGE_BATTERY',
                             ServiceState(
                                 'battery_simulator/set_battery_level',
                                 SetBatteryLevel,
                                 100,
                                 response_cb=self.recharge_cb),
                             transitions={'succeeded': ''})

            # Create the nav_patrol state machine using a Concurrence container
        self.nav_patrol = Concurrence(
            outcomes=['succeeded', 'recharge', 'stop'],
            default_outcome='succeeded',
            child_termination_cb=self.concurrence_child_termination_cb,
            outcome_cb=self.concurrence_outcome_cb)

        # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
        with self.nav_patrol:
            Concurrence.add('SM_NAV', self.sm_nav)
            # Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
            Concurrence.add(
                'LOCATION_GOAL',
                MonitorState("nav_location_goal", Pose,
                             self.nav_location_goal_cb))
            Concurrence.add(
                'RECOGNIZE_GOAL',
                MonitorState("/comm/msg/control/recognize_goal", String,
                             self.recognize_goal_cb))
            #Concurrence.add('RESULT', MonitorState("/drv_action/result", VisionActionResult, self.result_goal_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('PATROL',
                             self.nav_patrol,
                             transitions={
                                 'succeeded': 'PATROL',
                                 'recharge': 'RECHARGE',
                                 'stop': 'STOP'
                             })
            StateMachine.add('RECHARGE',
                             self.sm_recharge,
                             transitions={'succeeded': 'PATROL'})
            StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})

        rospy.loginfo('=============ce shi=============')
        time.sleep(5)
        # 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()
Пример #56
0
    def __init__(self, driver=None):
        StateMachine.__init__(self, outcomes=['succeeded',
                                              'preempted', 'aborted'])

        self.userdata.text = ''

        with self:

            StateMachine.add('PREPARE',
                             HomeOn_SM('Sit'),
                             transitions={'succeeded': 'INIT'})
                             #transitions={'succeeded': 'GAME'}) #DEBUGGING LINE
            #PREPARE NAO
            setup = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
            with setup:
                StateMachine.add('CLEAN_LISTEN_STATE',
                                 StopRecognitionState(),
                                 transitions={'succeeded': 'SET_VOCABULARY'}
                                 )
                StateMachine.add('SET_VOCABULARY',
                                 SetSpeechVocabularyState(no_words + yes_words + probably_not_words + dunno_words + maybe_words),
                                 transitions={'succeeded': 'RESET_AKINATOR'}
                                 )

                #RESET AKINATOR WEB
                StateMachine.add('RESET_AKINATOR',
                                 ServiceState('/reset_akinator_params',
                                              ResetAkinator,
                                              request=ResetAkinatorRequest('Name', 22)),
                                 transitions={'succeeded': 'succeeded'}
                                 )
            cc = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='aborted',
                             outcome_map={'succeeded': {'START_GAME_INTRO': 'succeeded', 'SETUP': 'succeeded'}})
            with cc:
                PresentationPool = ['CIR_Presentation1', 'CIR_Presentation2']
                # INTRODUCTION OF THE GAME
                Concurrence.add('START_GAME_INTRO',
                                ExecuteBehaviorFromPoolSM(behavior_pool=PresentationPool))
                # NAO SETUP
                Concurrence.add('SETUP', setup)

            StateMachine.add('INIT', cc,
                             transitions={'succeeded': 'GAME', 'aborted': 'LOSE'})

            #GAME LOOP
            StateMachine.add('GAME',
                             AkinatorGame(),
                             transitions={'succeeded': 'CHAR_QUESTION_MAKE', 'aborted': 'LOSE'}
                             )

            #END OF GAME
            StateMachine.add('CHAR_QUESTION_MAKE',
                             CBState(self.compoundQuestion,
                                     input_keys=['text'],
                                     output_keys=['text'],
                                     outcomes=['succeeded', 'aborted']),
                             transitions={'succeeded': 'CHAR_CORRECT'}
                             )

            StateMachine.add('CHAR_CORRECT',
                             SpeechGesture(behavior_pool=['CIR_Asking1', 'CIR_Asking2', 'CIR_Asking3', 'CIR_Asking4', 'CIR_Asking5', 'CIR_Asking6']),
                             transitions={'succeeded': 'FINAL_ANSWER'}
                             )
            StateMachine.add('FINAL_ANSWER',
                             SpeechRecognitionAndGesture(),
                             transitions={'succeeded': 'CHECK_WIN_LOSE', 'aborted': 'LOSE'}
                             )

            def check_w_l(userdata):
                if (userdata.text == 'yes'):
                    return 'win'
                return 'lose'
            StateMachine.add('CHECK_WIN_LOSE',
                             CBState(check_w_l, input_keys=['text'], outcomes=['win', 'lose']),
                             transitions={'win': 'WIN', 'lose': 'LOSE'})

            StateMachine.add('WIN',
                             ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Winning1', 'CIR_Winning2', 'CIR_Winning3', 'CIR_Winning4', 'CIR_Winning5']),
                             transitions={'succeeded': 'DISABLE_STIFF'})
            StateMachine.add('LOSE',
                             #ExecuteBehavior(behavior_name='CIR_Losing1'),
                             ExecuteBehaviorFromPoolSM(behavior_pool=['CIR_Loosing1', 'CIR_Loosing2', 'CIR_Loosing3', 'CIR_Loosing4']),
                             transitions={'succeeded': 'DISABLE_STIFF'})

            StateMachine.add('DISABLE_STIFF',
                             DisableStiffnessState(),
                             transitions={'succeeded': 'succeeded'})
    def __init__(self):
        rospy.init_node('carry_smach_move', anonymous=False)
        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        global recharge_position
        global main_room_position
        global corridor_position
        global kitchen_position
        global bed_room_position

        self.recharge_position = recharge_position
        self.main_room_position = main_room_position
        self.corridor_position = corridor_position
        self.kitchen_position = kitchen_position
        self.bed_room_position = bed_room_position


            #StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]),
            #                 transitions={'succeeded':'IN_KITCHEN',
            #                              'aborted':'aborted'})



	#!!!!!!!!!!!!!!!!!!!!!!!!!!!
	# NEW SM !!!!!!!!!!!!!!!!!!!
	#!!!!!!!!!!!!!!!!!!!!!!!!!!!



        ##################
        # IN CHARGE ROOM 
        ##################

	# Concurrent State machine 
        self.sm_in_charge_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_recharge','go_main_room'],
                                        output_keys=['sm_output'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_charge_room_wait_input_child_termination_cb,
                                        outcome_cb=self.in_charge_room_wait_input_outcome_cb)

        with self.sm_in_charge_room_wait_input:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
            Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))

	# Create state machine
        self.sm_in_charge_room = StateMachine(outcomes=['succeeded', 'go_main_room', 'preempted'],
                                              input_keys=['sm_input'],
                                              output_keys=['sm_output'])

        # Add 
        with self.sm_in_charge_room:
            StateMachine.add('CHECK_DESTINATION', CheckDestination(),
                             transitions={'succeeded':'WAIT_INPUT',
                                          'aborted':'WAIT_INPUT',
                                          'go_recharge':'STOP_NAV',
                                          'go_main_room':'WAIT_INPUT',
                                          'go_corridor':'WAIT_INPUT',
                                          'go_kitchen':'WAIT_INPUT',
                                          'go_bed_room':'WAIT_INPUT'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})  
            StateMachine.add('WAIT_INPUT', self.sm_in_charge_room_wait_input, transitions={'succeeded':'succeeded',
                                                                                'aborted':'succeeded',
                                                                                'go_recharge':'STOP_NAV',
                                                                                'go_main_room':'go_main_room'},
                                                                              remapping={'sm_output':'sm_output'})
            StateMachine.add('TEST', Pause2(),
                             transitions={'succeeded':'go_main_room',
                                          'aborted':'succeeded'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})

            StateMachine.add('STOP_NAV', PauseNav(), transitions={'succeeded':'GO_BACK'})
            StateMachine.add('GO_BACK', MoveBack(), transitions={'succeeded':'PAUSE_BACK'})
            StateMachine.add('PAUSE_BACK', Pause(), transitions={'succeeded':'STOP_BACK'})
            StateMachine.add('STOP_BACK', Stop(), transitions={'succeeded':'PAUSE_STOP'})
            StateMachine.add('PAUSE_STOP', Pause(), transitions={'succeeded':'CHARGING'})
            StateMachine.add('CHARGING', Charging(), transitions={'succeeded':'GO_FRONT'})
            StateMachine.add('GO_FRONT', MoveFront(), transitions={'succeeded':'PAUSE_FRONT'})
            StateMachine.add('PAUSE_FRONT', Pause(), transitions={'succeeded':'STOP_FRONT'})
            StateMachine.add('STOP_FRONT', Stop(), transitions={'succeeded':'START_NAV'})
            StateMachine.add('START_NAV', ResumeNav(), transitions={'succeeded':'WAIT_INPUT'})



        ##################
        # IN  MAIN  ROOM 
        ##################

        # Concurrent State machine 
        self.sm_in_main_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_recharge','go_corridor'],
                                        output_keys=['sm_output'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_main_room_wait_input_child_termination_cb,
                                        outcome_cb=self.in_main_room_wait_input_outcome_cb)

        with self.sm_in_main_room_wait_input:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
            Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))


        # Create state machine
        self.sm_in_main_room = StateMachine(outcomes=['succeeded', 'go_charge_room', 'go_corridor', 'preempted'],
                                            input_keys=['sm_input'],
                                            output_keys=['sm_output'])

        # Add 
        with self.sm_in_main_room:
            StateMachine.add('CHECK_DESTINATION', CheckDestination(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'succeeded',
                                          'go_recharge':'go_charge_room',
                                          'go_main_room':'WAIT_INPUT',
                                          'go_corridor':'go_corridor',
                                          'go_kitchen':'go_corridor',
                                          'go_bed_room':'go_corridor'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('TEST', Pause2(),
                             transitions={'succeeded':'go_corridor',
                                          'aborted':'succeeded'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('WAIT_INPUT', self.sm_in_main_room_wait_input, transitions={'succeeded':'WAIT_INPUT',
                                                                                'aborted':'WAIT_INPUT',
                                                                                'go_recharge':'go_charge_room',
                                                                                'go_corridor':'go_corridor'},
                                                                              remapping={'sm_output':'sm_output'})

        ##################
        # IN  CORRIDOR 
        ##################

        # Concurrent State machine 
        self.sm_in_corridor_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_main_room','go_kitchen','go_bed_room'],
                                        output_keys=['sm_output'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_corridor_wait_input_child_termination_cb,
                                        outcome_cb=self.in_corridor_wait_input_outcome_cb)

        with self.sm_in_corridor_wait_input:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
            Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))


        # Create state machine
        self.sm_in_corridor = StateMachine(outcomes=['succeeded', 'go_main_room', 'go_kitchen', 'go_bed_room', 'preempted'],
                                           input_keys=['sm_input'],
                                           output_keys=['sm_output'])

        # Add 
        with self.sm_in_corridor:
            StateMachine.add('CHECK_DESTINATION', CheckDestination(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'succeeded',
                                          'go_recharge':'go_main_room',
                                          'go_main_room':'go_main_room',
                                          'go_corridor':'WAIT_INPUT',
                                          'go_kitchen':'go_kitchen',
                                          'go_bed_room':'go_bed_room'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('TEST', Pause2(),
                             transitions={'succeeded':'go_kitchen',
                                          'aborted':'succeeded'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('WAIT_INPUT', self.sm_in_corridor_wait_input, transitions={'succeeded':'WAIT_INPUT',
                                                                                'aborted':'WAIT_INPUT',
                                                                                'go_main_room':'go_main_room',
                                                                                'go_kitchen':'go_kitchen',
                                                                                'go_bed_room':'go_bed_room'},
                                                                              remapping={'sm_output':'sm_output'})

        ##################
        # IN  KITCHEN 
        ##################


        # Concurrent State machine 
        self.sm_in_kitchen_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_corridor'],
                                        output_keys=['sm_output'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_kitchen_wait_input_child_termination_cb,
                                        outcome_cb=self.in_kitchen_wait_input_outcome_cb)

        with self.sm_in_kitchen_wait_input:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
            Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))


        # Create state machine
        self.sm_in_kitchen = StateMachine(outcomes=['succeeded', 'go_corridor', 'preempted'],
                                          input_keys=['sm_input'],
                                          output_keys=['sm_output'])

        # Add 
        with self.sm_in_kitchen:
            StateMachine.add('WAIT_INPUT', self.sm_in_kitchen_wait_input, transitions={'succeeded':'WAIT_INPUT',
                                                                                'aborted':'WAIT_INPUT',
                                                                                'go_corridor':'go_corridor'},
                                                                              remapping={'sm_output':'sm_output'})
            StateMachine.add('TEST', Pause2(),
                             transitions={'succeeded':'go_corridor',
                                          'aborted':'succeeded'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('CHECK_DESTINATION', CheckDestination(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'succeeded',
                                          'go_recharge':'go_corridor',
                                          'go_main_room':'go_corridor',
                                          'go_corridor':'go_corridor',
                                          'go_kitchen':'WAIT_INPUT',
                                          'go_bed_room':'go_corridor'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})


        ##################
        # IN  BED  ROOM 
        ##################


        # Concurrent State machine 
        self.sm_in_bed_room_wait_input = Concurrence(outcomes=['succeeded','aborted','preempted','go_corridor'],
                                        output_keys=['sm_output'],
                                        default_outcome='succeeded',
                                        child_termination_cb=self.in_bed_room_wait_input_child_termination_cb,
                                        outcome_cb=self.in_bed_room_wait_input_outcome_cb)

        with self.sm_in_bed_room_wait_input:
            Concurrence.add('GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb))
            Concurrence.add('GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb))
            Concurrence.add('GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb))
            Concurrence.add('GO_TO_CORRIDOR', MonitorState("/CARRY/go_corridor", Empty, self.empty_cb))
            Concurrence.add('GO_TO_RECHARGE', MonitorState("/CARRY/go_recharge", Empty, self.empty_cb))


        # Create state machine
        self.sm_in_bed_room = StateMachine(outcomes=['succeeded', 'go_corridor', 'preempted'],
                                           input_keys=['sm_input'],
                                           output_keys=['sm_output'])

        # Add 
        with self.sm_in_bed_room:
            StateMachine.add('WAIT_INPUT', self.sm_in_bed_room_wait_input, transitions={'succeeded':'WAIT_INPUT',
                                                                                'aborted':'WAIT_INPUT',
                                                                                'go_corridor':'go_corridor'},
                                                                              remapping={'sm_output':'sm_output'})
            StateMachine.add('TEST', Pause2(),
                             transitions={'succeeded':'go_corridor',
                                          'aborted':'succeeded'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})
            StateMachine.add('CHECK_DESTINATION', CheckDestination(),
                             transitions={'succeeded':'succeeded',
                                          'aborted':'succeeded',
                                          'go_recharge':'go_corridor',
                                          'go_main_room':'go_corridor',
                                          'go_corridor':'go_corridor',
                                          'go_kitchen':'go_corridor',
                                          'go_bed_room':'WAIT_INPUT'},
                             remapping={'waypoint_in':'sm_input',
                                        'waypoint_out':'sm_output'})



        ##################
        # TOP SM 
        ##################

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

        # Add nav_patrol, sm_recharge and a Stop() machine to sm_top
        with self.sm_top:
            StateMachine.add('IN_CHARGE_ROOM', self.sm_in_charge_room, transitions={'succeeded':'IN_CHARGE_ROOM',
                                                                                'go_main_room':'IN_MAIN_ROOM'},
                                                                       remapping={'sm_input':'goal', 'sm_output':'goal'})

            StateMachine.add('IN_MAIN_ROOM', self.sm_in_main_room, transitions={'succeeded':'IN_MAIN_ROOM',
                                                                                'go_charge_room':'IN_CHARGE_ROOM',
                                                                                'go_corridor':'IN_CORRIDOR'},
                                                                   remapping={'sm_input':'goal', 'sm_output':'goal'})

            StateMachine.add('IN_CORRIDOR', self.sm_in_corridor, transitions={'succeeded':'IN_CORRIDOR',
                                                                                'go_main_room':'IN_MAIN_ROOM',
                                                                                'go_kitchen':'IN_KITCHEN',
                                                                                'go_bed_room':'IN_BED_ROOM'},
                                                                 remapping={'sm_input':'goal', 'sm_output':'goal'})

            StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={'succeeded':'IN_KITCHEN',
                                                                                'go_corridor':'IN_CORRIDOR'},
                                                               remapping={'sm_input':'goal', 'sm_output':'goal'})

            StateMachine.add('IN_BED_ROOM', self.sm_in_bed_room, transitions={'succeeded':'IN_BED_ROOM',
                                                                                'go_corridor':'IN_CORRIDOR'},
                                                                 remapping={'sm_input':'goal', 'sm_output':'goal'})






        # Create and start the SMACH introspection server
        intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_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()
Пример #58
0
def main():
    rospy.init_node('tinker_mission_manipulation')
    trans = tf.TransformListener()
    rospy.loginfo("Waiting for tf ...")
    rospy.sleep(3)
    assert (len(trans.getFrameStrings()) > 0)

    state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with state:
        def kinect_callback(userdata, result):
            userdata.objects = []
            objects = []
            sum_x = 0
            for obj in result.objects.objects:
                position = obj.pose.pose.pose.position
                if position.y > 0.5 or position.y < -0.5:
                    continue
                _levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80] 
                for l in _levels:
                    if fabs(l - position.z) < 0.05:
                        position.z = l + 0.05
                if position.z > 1.1:
                    position.z += 0.05
                obj.header.stamp = rospy.Time(0)
                kinect_point = PointStamped(header=obj.header, point=position)
                odom_point = trans.transformPoint('odom', kinect_point)
                sum_x += odom_point.point.x
                rospy.loginfo(colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x,
                              odom_point.point.y, odom_point.point.z)
                objects.append(odom_point)

            avg_x = sum_x / len(objects)
            
            for from_point in objects:
                to_point = copy.deepcopy(from_point)
                to_point.point.x = avg_x 
                to_point.point.z = find_div(from_point.point.z)
                userdata.objects.append({'from': from_point, 'to': to_point})
                rospy.loginfo(colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x,
                              to_point.point.y, to_point.point.z)
            rospy.loginfo(colored('Total Object: %d','green'), len(objects))
            return 'succeeded'

        StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect), 
                transitions={'succeeded': 'Start_Button'})
        StateMachine.add('Start_Button', MonitorStartButtonState(), 
                transitions={'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition'})
        StateMachine.add('S_Kinect_Recognition',
                ServiceState(service_name='/kinect_find_objects',
                    service_spec=FindObjects,
                    input_keys=['objects'],
                    output_keys=['objects'],
                    response_cb=kinect_callback),
                transitions={'succeeded': 'Generate_Report'})
        StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'), 
                transitions={'succeeded': 'IT_Objects_Iterator'} )

        objects_iterator = Iterator(outcomes=['succeeded', 'preempted', 'aborted'],
                input_keys=['objects'], output_keys=[],
                it=lambda: state.userdata.objects, it_label='target',
                exhausted_outcome='succeeded')

        with objects_iterator:
            fetch_object_sequence = Sequence(outcomes=['succeeded', 'aborted', 'continue', 'preempted'],
                                             input_keys=['target'],
                                             connector_outcome='succeeded')
            with fetch_object_sequence:
                Sequence.add('Speak', SpeakState('New Object recognized'))
                Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN))
                Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'), 
                        transitions={'aborted':'continue'})
                concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
                                          default_outcome='succeeded',
                                          child_termination_cb=lambda x: True,
                                          input_keys=['target'])
                with concurrence:
                    Concurrence.add('Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from'))
                    Concurrence.add('Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x,y: False))

                Sequence.add('Move_Fetch_Concurrence', concurrence)
                Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE))
                Sequence.add('Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from'))
                Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to'))
                Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to'))
                Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN))
                Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'})

            Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue'])

        # end of objects_iterator
        StateMachine.add('IT_Objects_Iterator', objects_iterator,
                transitions= {'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset'})

        StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init),
                         transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})

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

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