Esempio n. 1
0
def runMachine(start):
    eventHub = core.EventHub()
    qeventHub = core.QueuedEventHub(eventHub)

    subsystems = core.SubsystemList()
    subsystems.append(qeventHub)
    machine = state.Machine(deps=subsystems)

    sequence = raw_input('Enter sequence: ')

    machine.start(start)
    assert machine.currentState() is not None, 'State machine not started'
    for x in sequence:
        eventType = globals()[x.upper()]
        qeventHub.publish(eventType, core.Event())
        qeventHub.publishEvents()

        assert machine.currentState(
        ) is not None, 'State machine ended prematurely'

    # State machine finished, send finish event
    qeventHub.publish(FINISH, core.Event())
    qeventHub.publishEvents()

    assert machine.currentState() is None, 'State machine has not ended'
    machine.stop()
Esempio n. 2
0
    def update(self, timestep):
        self._currentTime += timestep

        sinVal = math.sin(self._currentTime + self._offset)
        self.voltage = 2.0 * sinVal + 26
        self.current = 5.0 * sinVal + 5
        if sinVal >= 0:
            self.enabled = True
            self.publish(device.IPowerSource.ENABLED, core.Event())
        else:
            self.enabled = False
            self.publish(device.IPowerSource.DISABLED, core.Event())

        if math.sin(self._currentTime + self._offset + 1) >= 0:
            self.used = True
            self.publish(device.IPowerSource.USING, core.Event())
        else:
            self.used = False
            self.publish(device.IPowerSource.NOT_USING, core.Event())

        event = ext.math.NumericEvent()
        event.number = self.voltage
        self.publish(device.IVoltageProvider.UPDATE, event)

        event = ext.math.NumericEvent()
        event.number = self.current
        self.publish(device.ICurrentProvider.UPDATE, event)
Esempio n. 3
0
 def CUPID_FOUND(self, event):
     # grab the color, fire the appropriate torpedo
     if event.color == vision.Color.ColorType.BLUE:
         self.vehicle.fireTorpedo(1)
         self.publish(Fire2.FIRED, core.Event())
     elif event.color == vision.Color.ColorType.RED:
         self.vehicle.fireTorpedo(2)
         self.publish(Fire2.FIRED, core.Event())
Esempio n. 4
0
 def TIMEOUT(self, event):
     # Figure out what state it was in when it timed out
     if self._state == StagedTemplate.SEARCHING:
         self.publish(StagedTemplate.EARLY_TIMEOUT, core.Event())
     elif self._state == StagedTemplate.LOST:
         self.publish(StagedTemplate.LOST_TIMEOUT, core.Event())
     else:
         # Normal timeout is the default if an improper state was set
         self.publish(StagedTemplate.NORMAL_TIMEOUT, core.Event())
Esempio n. 5
0
    def enter(self):
        # Interstate data is stored here
        # All previous data will be lost
        self.ai.data['light'] = {}

        # TODO: Make 'buoy' a configurable value
        if self.vehicle.hasObject('buoy'):
            self.publish(Start.SUCCESS, core.Event())
        else:
            self.publish(Start.FAIL, core.Event())
Esempio n. 6
0
    def testSubscribeToType(self):
        recv = Reciever()
        self.ehub.subscribeToType("Type", recv)
        self.assertEquals(0, recv.calls)

        self.epubA.publish("Type", core.Event())
        self.assertEquals(1, recv.calls)

        self.epubB.publish("Type", core.Event())
        self.assertEquals(2, recv.calls)
Esempio n. 7
0
    def testSubscrie(self):
        recv = Reciever()
        self.qehub.subscribe('A', self.epubA, recv)

        self.assertEquals(0, recv.calls)
        self.epubB.publish('A', core.Event())
        self.assertEquals(0, recv.calls)

        self.epubA.publish('A', core.Event())
        self.assertEquals(0, recv.calls)
        self.qehub.publishEvents()
        self.assertEquals(1, recv.calls)
Esempio n. 8
0
    def testSubscribe(self):
        recvA = Reciever()
        recvB = Reciever()
        self.ehub.subscribe("Type", self.epubA, recvA)
        self.ehub.subscribe("Type", self.epubB, recvB)

        self.epubA.publish("Type", core.Event())
        self.assertEquals(1, recvA.calls)
        self.assertEquals(0, recvB.calls)

        self.epubB.publish("Type", core.Event())
        self.assertEquals(1, recvA.calls)
        self.assertEquals(1, recvB.calls)
Esempio n. 9
0
    def enter(self, timeout = 2):
        # Turn off all motions, hold the current heading
        self.motionManager.stopCurrentMotion()
        self.controller.holdCurrentHeading()
        if self._config.get('holdDepth', False):
            self.controller.holdCurrentDepth()
        self.controller.setSpeed(0)
        self.controller.setSidewaysSpeed(0)

        # Create a timer event
        self._timeout = self._config.get('timeout', timeout)
        # Timer will only state if the timeout is a positive number
        # A timer of 0 will turn it off, along with any negative number
        if self._timeout > 0:
            self.timer = \
                self.timerManager.newTimer(FindAttempt.TIMEOUT, self._timeout)
            self.timer.start()
        elif self._timeout < 0:
            # A negative timer will automatically move to the timeoutState
            self.timer = None
            self.publish(FindAttempt.TIMEOUT, core.Event())
        else:
            # A timer of zero will turn off the timer and will only allow
            # FindAttempt to exit by finding the target
            self.timer = None
Esempio n. 10
0
    def FINISHED(self, event):

        #Changes whether the robot strafes or moves forward
        if (self._stateOfMovement >= 1):
            self._stateOfMovement = 0
        else:
            self._stateOfMovement = self._stateOfMovement + 1

        #Making sure the robot doesn't go past the height bounding box
        if (self._tdt < self._boundingHeight):

            if (self._stateOfMovement == 0):
                self.Forward()

            else:

                #Switch the direction of the strafe
                if (self._strafeDirection == 1):
                    self._strafeDirection = -1
                else:
                    self._strafeDirection = 1

                if (self._firstMovement == 1):  #Will only happen once
                    self._firstMovement = 5
                    self.Strafe(self._strafeDirection, 2)
                else:
                    self.Strafe(self._strafeDirection, 1)

        #end the program
        else:
            self.publish(ZigZag.DONE, core.Event())
Esempio n. 11
0
 def _finish(self):
     """
     Raises, motion finished event.
     """
     event = core.Event()
     event.motion = self
     self.publish(Motion.FINISHED, event)
Esempio n. 12
0
    def testLookupByName(self):
        self.called = False
        def recieve(event):
            self.called = True

        # Create a publisher with an event hub
        eventHub = core.EventHub()
        mypub = core.EventPublisher(eventHub = eventHub,
                                    name = TestEventPublisher.EVTP_NAME)

        # Look it up and make sure it works
        epub = core.EventPublisher.lookupByName(TestEventPublisher.EVTP_NAME)
        self.assertNotEquals(None, epub)
        self.assertEquals(TestEventPublisher.EVTP_NAME, epub.getPublisherName())

        # Subscribe to the new one (which is wrapper of the same underlying
        # C++ object as self.epub)
        # BUG: This line below should work instead of the one two down
#        eventHub.subscribe("TestEvent", epub, recieve)
        eventHub.subscribe("TestEvent", mypub, recieve)

        # Send through our local
        mypub.publish("TestEvent", core.Event())

        # Make sure we got it
        self.assert_(self.called)
Esempio n. 13
0
    def WINDOW_FOUND(self, event):
        """Update the state of the target, this moves the vehicle"""
        # Check if this event should be vetoed, if it should, pass it on
        ret = WindowTrackingState.WINDOW_FOUND(self, event)
        if ret is False and not self._average:
            return ret

        # Average the found windows if specified
        if self._average:
            x, y, range, squareNess = self.mergeWindows(event)
        else:
            x, y, range, squareNess = \
                event.x, event.y, event.range, event.squareNess

        self._updateFilters(x, y, range, squareNess)
        
        y = self._filterdY
        if 0 == self._depthGain:
             y = 0
        
        # We ignore azimuth and elevation because we aren't using them
        self._target.setState(0, 0, self._filterdRange, self._filterdX, self._filterdY, event.timeStamp)
        
        # Only triggered the in range event if we are close and the target is
        # centered in the field of view
        rangeError = pmath.fabs(self._filterdRange - self._desiredRange)
        frontDistance = pmath.sqrt(self._filterdX ** 2 + y ** 2)
        if (rangeError < self._rangeThreshold) and \
            (frontDistance < self._frontThreshold):
            self.publish(SeekingToRange.IN_RANGE, core.Event())

        return ret
Esempio n. 14
0
    def BARBED_WIRE_FOUND(self, event):
        """Update the state of the target, this moves the vehicle"""
        # Todo: consider filter removal
        #self._updateFilters(event)

        x = event.topX
        y = event.topY
        width = event.topWidth

        # Determine y value based on whether we are ignoring depth or not,
        # also store this event
        StoreBarbedWireEvent.BARBED_WIRE_FOUND(self, event)
        if self._depthGain == 0:
            y = 0

        # Width == 1 when we are close, so inversion is needed to get range
        range = 1 - width

        # Finally set the state (We ignore azimuth and elevation because we
        # aren't using them)
        self._target.setState(0,
                              0,
                              range=range,
                              x=x,
                              y=y,
                              timeStamp=event.timeStamp)

        # Only triggered the in range event if we are close and the target is
        # centered in the field of view
        rangeError = math.fabs(range - self._desiredRange)
        frontDistance = math.sqrt(x**2 + y**2)
        if (rangeError < self._rangeThreshold) and \
            (frontDistance < self._frontThreshold):
            self.publish(SeekingToRange.IN_RANGE, core.Event())
Esempio n. 15
0
    def BUOY_FOUND(self, event):
        if(self.STEPNUM == 0):
            print("Window X: " + str(event.x))
            if(event.x <= self._xmin):
                print("Moving left to compensate")
                self.move(-self._distance)
            elif(event.x >= self._xmax):
                print("Moving right to compensate")
                self.move(self._distance)

            self.STEPNUM += 1
        
        elif(self.STEPNUM == 1):
            if(event.x > self._xmin and event.x < self._xmax):
                print("X Axis Aligned")
                self.motionManager.stopCurrentMotion()
                self.STEPNUM += 1

        elif(self.STEPNUM == 2):
            print("Window Y: " + str(event.y))
            if(event.y <= self._ymin):
                print("Moving down to compensate")
                self.dive(self._distance)
            elif(event.y >= self._ymax):
                print("Moving up to compensate")
                self.dive(-self._distance)

            self.STEPNUM += 1

        elif(self.STEPNUM == 3):
            if(event.y > self._ymin and event.y < self._ymax):
                print("Y Axis Aligned, All Done")
                self.motionManager.stopCurrentMotion()
                self.publish(Center.CENTERED, core.Event())
Esempio n. 16
0
    def BIN_FOUND(self, event):
        HoveringState.BIN_FOUND(self, event)

        # Fire event if we are centered over the bin
        if self._currentBin(event): 
            if math.Vector2(event.x, event.y).length() < self._centeredRange:
                self.publish(BinSortingState.CENTERED_, core.Event())
Esempio n. 17
0
    def _enterState(self, newStateClass):
        """
        Does all the house keeping when entering a new state
        """
        
        # Look up config based on full dotted name of state class
        fullClassName = '%s.%s' % (newStateClass.__module__, 
                                   newStateClass.__name__)
        stateCfg = self._config.get('States', {})

        # Get the superclasses of the state
        #stateList = resolve(fullClassName).__mro__

        # Load the superclass config values, skipping object
        #config = {}
        #for index in xrange(len(stateList) - 2, 0, -1):
            #print "loading", stateList[index]
        #    config.update(stateCfg.get(stateList[index], {}))

        # Load this states config values
        #config.update(stateCfg.get(fullClassName, {}))
        config = stateCfg.get(fullClassName, {})
        
        # Add self to the list of subsystems
        subsystems = self._subsystems
        name = self.getName()
        subsystems[name[0].lower() + name[1:]] = self
        
        # Create state instance from class, make sure to pass all subsystems
        # along as well
        newState = newStateClass(config, **self._subsystems)
        newState.publish = self.publish
        
        # Subscribe to every event of the desired type
        transitionTable = newState.transitions()
        if self._qeventHub is not None:
            for eventType in transitionTable.iterkeys():
                if type(eventType) == type(self._enterState):
                    raise Exception("Event type is actually a function")
                else:
                    conn = self._qeventHub.subscribeToType(eventType, 
                                                           self.injectEvent)
                    self._connections.append(conn)
        
        # Actual enter the state and record it as our new current state
        self._currentState = newState
        self._currentState.enter()
        
        # Notify everyone we just entered the state
        event = core.StringEvent()
        event.string = fullClassName
        self.publish(Machine.STATE_ENTERED, event)
        
        # If we are in a state with no way out, exit the state and mark ourself
        # complete
        if 0 == len(transitionTable):
            self._exitState()
            self._complete = True
            self.publish(Machine.COMPLETE, core.Event())
Esempio n. 18
0
    def BARBED_WIRE_FOUND(self, event):
        # Publish aligned event if needed
        alignment = math.fabs(event.topX) + math.fabs(event.bottomX)
        if alignment < self._minAlignment:
            self.publish(SeekingToAligned.ALIGNED, core.Event())

        # Update motion
        TargetAlignState.BARBED_WIRE_FOUND(self, event)
Esempio n. 19
0
    def BUOY_FOUND(self, event):
        """Update the state of the buoy, this moves the vehicle"""
        ret = BuoyTrackingState.BUOY_FOUND(self, event)
        if ret is False:
            return ret

        self._buoy.setState(event.azimuth, event.elevation, event.range,
                            event.x, event.y, event.timeStamp)

        change = self._buoy.changeOverTime()
        if self._compareChange((event.x, event.y), (change[3], change[4])):
            # Only applies if depthGain is set to 0
            if abs(event.y) > self._planeThreshold:
                # Need to recorrect the height
                self.publish(Align.INCORRECT_DEPTH, core.Event())
            else:
                # We are properly aligned
                self.publish(Align.SEEK_BUOY, core.Event())
Esempio n. 20
0
    def LOVERSLANE_FOUND(self, event):
        if pmath.fabs(event.centerX) < self._centerBoundary and \
                pmath.fabs(event.centerY) < self._centerBoundary:
            self.publish(Aligning.CENTERED, core.Event())
            return

        self._lane.setState(event.centerX * self._sidewaysSpeed,
                            event.centerY * self._diveSpeed)
        self.changedLane()
Esempio n. 21
0
    def BARBED_WIRE_FOUND(self, event):
        TargetAlignState.BARBED_WIRE_FOUND(self, event)

        if event.bottomWidth != -1:
            if (math.fabs(event.topX) < self._threshold) and \
                    (math.fabs(event.bottomX) < self._threshold):
                if self.timer is not None:
                    self.timer.stop()
                self.publish(Aligning.SETTLED, core.Event())
Esempio n. 22
0
    def enter(self):
        # If the offset values exist, delete them
        if self.ai.data.has_key('dive_offsetTheOffset'):
            del self.ai.data['dive_offsetTheOffset']
        if self.ai.data.has_key('closerlook_offsetTheOffset'):
            del self.ai.data['closerlook_offsetTheOffset']

        self.visionSystem.binDetectorOff()
        self.publish(COMPLETE, core.Event())
Esempio n. 23
0
 def _startQueuedMotion(self):
     # Check if there are any queued motions
     if len(self._queuedMotions) == 0:
         # Publish the queued event finish if there's no queued motions
         self.publish(MotionManager.FINISHED, core.Event())
     else:
         # Start the queued motion
         motion = self._queuedMotions.pop(0)
         self._setMotion(motion)
Esempio n. 24
0
    def LIGHT_FOUND(self, event):
        """Update the state of the light, this moves the vehicle"""
        StoreLightEvent.LIGHT_FOUND(self, event)
        self._light.setState(event.azimuth, event.elevation, event.range,
                             event.x, event.y, event.timeStamp)

        change = self._light.changeOverTime()
        if self._compareChange((event.x, event.y), (change[3], change[4])):
            self.publish(Align.SEEK_LIGHT, core.Event())
Esempio n. 25
0
    def BUOY_FOUND(self, event):
        ret = BuoyTrackingState.BUOY_FOUND(self, event)
        if ret is False:
            return ret

        if self._recoverMethod == "Close Range":
            # Turn off the timer and backwards motion
            if self.timer is not None:
                self.timer.stop()
                self.timer = None

                self.motionManager._stopMotion(self._recoverMotion)

                # Create the depth motion if needed
                newDepth = self.vehicle.getDepth()
                changeDepth = False

                if event.y > self._yThreshold:
                    newDepth = newDepth - self._closeDepthChange
                    changeDepth = True
                elif event.y < (0.0 - self._yThreshold):
                    newDepth = newDepth + self._closeDepthChange
                    changeDepth = True

                # Start the depth motion if necessary
                if changeDepth:
                    self._diveMotion = motion.basic.RateChangeDepth(
                        desiredDepth=newDepth, speed=self._diveSpeed)
                    self.motionManager.setMotion(self._diveMotion)
                    self._finished = False
                else:
                    self._finished = True

            # Check if the motion is finished
            if self._finished:
                self.publish(Recover.REFOUND_BUOY, core.Event())

            # Check if we should finish it early
            if ((0.0 - self._yThreshold) < event.y < self._yThreshold):
                self.motionManager.stopCurrentMotion()
                self.controller.holdCurrentDepth()
                self.publish(Recover.REFOUND_BUOY, core.Event())
        else:
            self.publish(Recover.REFOUND_BUOY, core.Event())
Esempio n. 26
0
    def testMultiple(self):
        recv = Reciever()
        types = ['A', 'B', 'C', 'D', 'E']
        for t in types:
            self.qehub.subscribeToType(t, recv)
        self.assertEquals(0, recv.calls)

        self.epubA.publish("A", core.Event())
        self.epubA.publish("B", core.Event())
        self.epubB.publish("C", core.Event())
        self.epubB.publish("D", core.Event())
        self.epubA.publish("E", core.Event())
        self.assertEquals(0, recv.calls)

        self.qehub.publishEvents()
        self.assertEquals(5, recv.calls)

        for expType, actType in zip(types, recv.etypes):
            self.assertEquals(expType, actType)
Esempio n. 27
0
    def enter(self, defaultTimeout=60):
        timeout = self.ai.data['config'].get('RandomBin',
                                             {}).get('taskTimeout',
                                                     defaultTimeout)
        task.Task.enter(self, defaultTimeout=timeout)

        if self.ai.data.get('binComplete', False):
            self.publish(RandomBin.MOVE_ON, core.Event())
        else:
            self.stateMachine.start(state.Branch(randombin.Start))
Esempio n. 28
0
 def BUOY_FOUND(self, event):
     color = str(event.color).lower()
     if (color in self.ai.data['buoyData']):
         self.ai.data['buoyData'][color].append(
             self.stateEstimator.getEstimatedPosition())
         self.ai.data[color + 'FoundNum'] += 1
         if (self.ai.data[color + 'FoundNum'] >= self._foundMin
                 and not self.ai.data['ignoreBuoy'][color]):
             self.ai.data['buoyColor'] = color
             self.publish(Strafe.FOUND_BUOY, core.Event())
Esempio n. 29
0
 def UPDATE(self, event):
     PingerState.UPDATE(self, event)
     if self._isNewPing(event):
         # Converting from the vehicle reference frame, to the image space
         # reference frame used by the pipe motion
         self._pipe.setState(-event.direction.y, event.direction.x, 
                             ext.math.Degree(0), event.timeStamp)
         
         if math.fabs(event.direction.z) > math.fabs(self._closeZ):
              self.publish(TranslationSeeking.CLOSE, core.Event())
Esempio n. 30
0
    def update(self, timestep):
        self._currentTime += timestep

        # Depth
        self.depth = 10 * math.sin(self._currentTime) + 10
        event = core.Event()
        event.number = self.depth
        self.publish(vehicle.IVehicle.DEPTH_UPDATE, event)

        # Orientation
        x = 1.0 * math.sin(self._currentTime) + 1.0
        y = 1.0 * math.sin(self._currentTime + 5) + 1.0
        z = 1.0 * math.sin(self._currentTime + 10) + 1.0
        vector = ext.math.Vector3(x, y, z)
        vector.normalise()

        angle = 2 * math.pi * math.sin(self._currentTime + 15) + 1.0
        self._orientation.FromAngleAxis(ext.math.Radian(angle), vector)

        self._orientation.normalise()
        event = core.Event()
        event.orientation = self._orientation
        self.publish(vehicle.IVehicle.ORIENTATION_UPDATE, event)

        # Position Update
        x = 1.0 * math.sin(self._currentTime) + 1.0
        y = 1.0 * math.sin(self._currentTime + 5) + 1.0

        event = ext.math.Vector2Event()
        event.vector2 = ext.math.Vector2(x, y)
        self.publish(vehicle.IVehicle.POSITION_UPDATE, event)

        # Velocity Update
        x = 1.0 * math.sin(self._currentTime + 10) + 1.0
        y = 1.0 * math.sin(self._currentTime + 15) + 1.0

        event = ext.math.Vector2Event()
        event.vector2 = ext.math.Vector2(x, y)
        self.publish(vehicle.IVehicle.VELOCITY_UPDATE, event)

        # Update Devices
        for device in self._devices.itervalues():
            device.update(timestep)