Exemplo n.º 1
0
    def build(self):
        """    
            Builds the mission behaviour
        """
        # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
        autonomous = smach.Concurrence(
            outcomes=["exitAutomode", "aborted"],
            default_outcome="exitAutomode",
            outcome_map={
                "exitAutomode": {"HMI": "preempted", "MEASURE": "preempted"},
                "aborted": {"HMI": "preempted", "MEASURE": "aborted"},
            },
            child_termination_cb=onPreempt,
        )
        with autonomous:
            smach.Concurrence.add("HMI", wii_states.interfaceState(self.hmi))
            smach.Concurrence.add("MEASURE", measure_point.build(self.point_list))

        # Build the top level mission control from the remote control state and the autonomous state
        mission_control = smach.StateMachine(outcomes=["preempted", "aborted"])
        with mission_control:
            smach.StateMachine.add(
                "REMOTE_CONTROL",
                wii_states.remoteControlState(self.hmi),
                transitions={"enterAutomode": "AUTO_MODE", "preempted": "preempted"},
            )
            smach.StateMachine.add(
                "AUTO_MODE", autonomous, transitions={"exitAutomode": "REMOTE_CONTROL", "aborted": "REMOTE_CONTROL"}
            )
        return mission_control
Exemplo n.º 2
0
    def build(self):
        # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
        autonomous = smach.Concurrence(outcomes=['exitAutomode'],
                                       default_outcome='exitAutomode',
                                       outcome_map={
                                           'exitAutomode': {
                                               'HMI': 'preempted',
                                               'MEASURE': 'preempted'
                                           }
                                       },
                                       child_termination_cb=onPreempt)
        with autonomous:
            smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
            smach.Concurrence.add('MEASURE', measure_point.build())

        # Build the top level mission control from the remote control state and the autonomous state
        mission_control = smach.StateMachine(outcomes=['preempted'])
        with mission_control:
            smach.StateMachine.add('REMOTE_CONTROL',
                                   wii_states.remoteControlState(self.hmi),
                                   transitions={
                                       'enterAutomode': 'AUTO_MODE',
                                       'preempted': 'preempted'
                                   })
            smach.StateMachine.add(
                'AUTO_MODE',
                autonomous,
                transitions={'exitAutomode': 'REMOTE_CONTROL'})
        return mission_control
Exemplo n.º 3
0
 def build(self):
      # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
     autonomous = smach.Concurrence(  outcomes = ['exitAutomode'],
                                             default_outcome = 'exitAutomode',
                                             outcome_map = {'exitAutomode':{'HMI':'preempted','MEASURE':'preempted'}},
                                             child_termination_cb = onPreempt)
     with autonomous:
         smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
         smach.Concurrence.add('MEASURE', measure_point.build())
     
     # Build the top level mission control from the remote control state and the autonomous state
     mission_control = smach.StateMachine(outcomes=['preempted'])            
     with mission_control:
         smach.StateMachine.add('REMOTE_CONTROL', wii_states.remoteControlState(self.hmi), transitions={'enterAutomode':'AUTO_MODE','preempted':'preempted'})
         smach.StateMachine.add('AUTO_MODE', autonomous, transitions={'exitAutomode':'REMOTE_CONTROL'})
     return mission_control
 def build(self):
     
     auto = self.build_row_nav()
     
     autonomous = smach.Concurrence(  outcomes = ['exitAutomode','aborted'],
                                             default_outcome = 'exitAutomode',
                                             outcome_map = {'exitAutomode':{'HMI':'preempted','ROWNAV':'preempted'},
                                                            'aborted':{'HMI':'preempted','ROWNAV':'aborted'}},
                                             child_termination_cb = lambda ud: True )
     with autonomous:
         smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
         smach.Concurrence.add('ROWNAV', auto)
     
     # Build the top level mission control from the remote control state and the autonomous state
     mission_control = smach.StateMachine(outcomes=['preempted','aborted'])            
     with mission_control:
         smach.StateMachine.add('REMOTE_CONTROL', wii_states.remoteControlState(self.hmi), transitions={'enterAutomode':'AUTO_MODE','preempted':'preempted'})
         smach.StateMachine.add('AUTO_MODE', autonomous, transitions={'exitAutomode':'REMOTE_CONTROL','aborted':'REMOTE_CONTROL'})
     return mission_control
    def build(self):

        auto = self.build_row_nav()

        autonomous = smach.Concurrence(outcomes=['exitAutomode', 'aborted'],
                                       default_outcome='exitAutomode',
                                       outcome_map={
                                           'exitAutomode': {
                                               'HMI': 'preempted',
                                               'ROWNAV': 'preempted'
                                           },
                                           'aborted': {
                                               'HMI': 'preempted',
                                               'ROWNAV': 'aborted'
                                           }
                                       },
                                       child_termination_cb=lambda ud: True)
        with autonomous:
            smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
            smach.Concurrence.add('ROWNAV', auto)

        # Build the top level mission control from the remote control state and the autonomous state
        mission_control = smach.StateMachine(outcomes=['preempted', 'aborted'])
        with mission_control:
            smach.StateMachine.add('REMOTE_CONTROL',
                                   wii_states.remoteControlState(self.hmi),
                                   transitions={
                                       'enterAutomode': 'AUTO_MODE',
                                       'preempted': 'preempted'
                                   })
            smach.StateMachine.add('AUTO_MODE',
                                   autonomous,
                                   transitions={
                                       'exitAutomode': 'REMOTE_CONTROL',
                                       'aborted': 'REMOTE_CONTROL'
                                   })
        return mission_control
Exemplo n.º 6
0
    def build(self):
        # Build concurrent route following and mine detection
        mine_search = smach.Concurrence ( outcomes = ['mineDetected','preempted'], 
                                       default_outcome = 'preempted',
                                       outcome_map = {'preempted':{'follow_route':'preempted'}, 'mineDetected':{'mine_detect':'invalid'}},
                                       output_keys=['next_x','next_y'],
                                       child_termination_cb = onPreempt)

        with mine_search:
            smach.Concurrence.add('follow_route', follow_route.build())
            smach.Concurrence.add('mine_detect', smach_ros.MonitorState("/wads", Float64, mine_detect_cb))

        # Build the demining task        
        demining = smach.StateMachine(outcomes=['success', 'preempted', 'aborted'])
        with demining:
            smach.StateMachine.add('mine_search', mine_search, transitions={'mineDetected':'mine_inspection'})
            smach.StateMachine.add('mine_inspection', mine_simple_avoidance.build() , transitions={'inspectionDone':'mine_search'})

        # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
        autonomous = smach.Concurrence(  outcomes = ['exitAutomode'],
                                                default_outcome = 'exitAutomode',
                                                outcome_map = {'exitAutomode':{'HMI':'preempted','demining':'preempted'}},
                                                child_termination_cb = onPreempt)

        with autonomous:
            smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
            smach.Concurrence.add('demining', demining)


        
        # Build the top level mission control from the remote control state and the autonomous state
        mission_control = smach.StateMachine(outcomes=['preempted'])            
        with mission_control:
            smach.StateMachine.add('REMOTE_CONTROL', wii_states.remoteControlState(self.hmi), transitions={'enterAutomode':'AUTO_MODE','preempted':'preempted'})
            smach.StateMachine.add('AUTO_MODE', autonomous, transitions={'exitAutomode':'REMOTE_CONTROL'})
        return mission_control
Exemplo n.º 7
0
    def build(self):
        # Build concurrent route following and mine detection
        mine_search = smach.Concurrence(outcomes=['mineDetected', 'preempted'],
                                        default_outcome='preempted',
                                        outcome_map={
                                            'preempted': {
                                                'follow_route': 'preempted'
                                            },
                                            'mineDetected': {
                                                'mine_detect': 'invalid'
                                            }
                                        },
                                        output_keys=['next_x', 'next_y'],
                                        child_termination_cb=onPreempt)

        with mine_search:
            smach.Concurrence.add('follow_route', follow_route.build())
            smach.Concurrence.add(
                'mine_detect',
                smach_ros.MonitorState("/wads", Float64, mine_detect_cb))

        # Build the demining task
        demining = smach.StateMachine(
            outcomes=['success', 'preempted', 'aborted'])
        with demining:
            smach.StateMachine.add(
                'mine_search',
                mine_search,
                transitions={'mineDetected': 'mine_inspection'})
            smach.StateMachine.add(
                'mine_inspection',
                mine_simple_avoidance.build(),
                transitions={'inspectionDone': 'mine_search'})

        # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption
        autonomous = smach.Concurrence(outcomes=['exitAutomode'],
                                       default_outcome='exitAutomode',
                                       outcome_map={
                                           'exitAutomode': {
                                               'HMI': 'preempted',
                                               'demining': 'preempted'
                                           }
                                       },
                                       child_termination_cb=onPreempt)

        with autonomous:
            smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi))
            smach.Concurrence.add('demining', demining)

        # Build the top level mission control from the remote control state and the autonomous state
        mission_control = smach.StateMachine(outcomes=['preempted'])
        with mission_control:
            smach.StateMachine.add('REMOTE_CONTROL',
                                   wii_states.remoteControlState(self.hmi),
                                   transitions={
                                       'enterAutomode': 'AUTO_MODE',
                                       'preempted': 'preempted'
                                   })
            smach.StateMachine.add(
                'AUTO_MODE',
                autonomous,
                transitions={'exitAutomode': 'REMOTE_CONTROL'})
        return mission_control