def testSleep(self): # Mock Nanosleep so we don't really have to sleep originalNanosleep = timer.nanosleep timer.nanosleep = self.mockNanosleep startTime = timer.time() timer.sleep(0.25) endTime = timer.time() timer.nanosleep = originalNanosleep self.assertEquals(0, self.sec) self.assertEquals(0.25 * 1e9, self.nsec)
def _recordEvent(self, event): if self._firstRun: self._startTime = timer.time() self._firstRun = False newEvent = RemoteAi.StateEvent() # Explicit cast necessary for Ice newEvent.timeStamp = long(event.timeStamp - self._startTime) newEvent.eventType = event.type # Find the state name fullClassName = str(event.string) # Recreate the class stateClass = resolve(fullClassName) if issubclass(stateClass, task.Task): # It's a task, make that the type newEvent.type = "Task" else: newEvent.type = "State" # Parse out the last name from the fullClassName newEvent.name = fullClassName.split('.')[-1] # Record this event in the adapter self._obj.RecordEvent(newEvent)
def start(self, controller, vehicle, estimator, eventHub, eventPublisher): """ Called by the motion manager to state main state variables DO NOT OVERRIDE this method directly. Override _start instead. @type controller: ext.control.IController @param controller: The current controller of the vehicle @type vehicle: ext.vehicle.IVehicle @param vehicle: The vehicle we are controlling @type eventHub: ext.core.QueuedEventHub @param eventHub: The event hub to subscribe to events through """ self._controller = controller self._vehicle = vehicle self._estimator = estimator self._eventHub = eventHub self._eventPublisher = eventPublisher self._startTime = timer.time() # Set up the publish method to be seemless and easy self.publish = self._eventPublisher.publish self._start()
def _update(self, event): # figure out at what time to evaluate the trajectory if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newOrientation = self._trajectory.computeValue(currentTime) newAngularRate = self._trajectory.computeDerivative(currentTime,1) # send the new values to the controller self._controller.rotate(newOrientation, newAngularRate) if (currentTime >= self._trajectory.getFinalTime() and self._controller.atOrientation()): self._finish()
def _update(self, event): # figure out at what time to evaluate the trajectory if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newOrientation = self._trajectory.computeValue(currentTime) newAngularRate = self._trajectory.computeDerivative(currentTime, 1) # send the new values to the controller self._controller.rotate(newOrientation, newAngularRate) if (currentTime >= self._trajectory.getFinalTime() and self._controller.atOrientation()): self._finish()
def __init__(self, cfg=None, deps=None): if deps is None: deps = [] if cfg is None: cfg = {} core.Subsystem.__init__(self, cfg.get("name", "CpuMonitor"), deps) self._qeventHub = core.Subsystem.getSubsystemOfType(core.QueuedEventHub, deps, nonNone=True) self._config = cfg self._bufSize = cfg.get("bufferSize", 1) logName = cfg.get("log_name", None) self._log = cfg.get("log_results", True) if self._log: if logName is None: # Generate log name timeStamp = datetime.fromtimestamp(timer.time()) directory = timeStamp.strftime("%Y%m%d%H%M%S") # Create the directory, the directory name # should never conflict. If it does, this should crash os.mkdir(directory) self._file = open(directory + "/cpu.log", "w") else: # A specified file must have the directory exist self._file = open(logName, "w") else: self._file = None # Setup logfile self._data = CpuData(self._file, self._bufSize) # Initial values f = open("/proc/stat") self._start_values = f.readline().split()[1:] self._start_time = time.time() f.close() # Pack the update types into an array self._updateTypes = [ CpuMonitor.USER_UPDATE, CpuMonitor.NICE_UPDATE, CpuMonitor.SYS_UPDATE, CpuMonitor.IDLE_UPDATE, CpuMonitor.IOWT_UPDATE, CpuMonitor.IRQ_UPDATE, CpuMonitor.SIRQ_UPDATE, ]
def __init__(self, cfg=None, deps=None): if deps is None: deps = [] if cfg is None: cfg = {} core.Subsystem.__init__(self, cfg.get('name', 'CpuMonitor'), deps) self._qeventHub = \ core.Subsystem.getSubsystemOfType(core.QueuedEventHub, deps, nonNone = True) self._config = cfg self._bufSize = cfg.get('bufferSize', 1) logName = cfg.get('log_name', None) self._log = cfg.get('log_results', True) if self._log: if logName is None: # Generate log name timeStamp = datetime.fromtimestamp(timer.time()) directory = timeStamp.strftime("%Y%m%d%H%M%S") # Create the directory, the directory name # should never conflict. If it does, this should crash os.mkdir(directory) self._file = open(directory + '/cpu.log', 'w') else: # A specified file must have the directory exist self._file = open(logName, 'w') else: self._file = None # Setup logfile self._data = CpuData(self._file, self._bufSize) # Initial values f = open('/proc/stat') self._start_values = f.readline().split()[1:] self._start_time = time.time() f.close() # Pack the update types into an array self._updateTypes = [ CpuMonitor.USER_UPDATE, CpuMonitor.NICE_UPDATE, CpuMonitor.SYS_UPDATE, CpuMonitor.IDLE_UPDATE, CpuMonitor.IOWT_UPDATE, CpuMonitor.IRQ_UPDATE, CpuMonitor.SIRQ_UPDATE ]
def _update(self, event): # figure out at what time to evaluate the trajectory if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newPosition = self._trajectory.computeValue(currentTime) newVelocity = self._trajectory.computeDerivative(currentTime,1) newAccel = self._trajectory.computeDerivative(currentTime, 2) if self._frame is Frame.LOCAL: # interpret the trajectory output as being in the local coordinate orientation = self._controller.getDesiredOrientation() yaw = orientation.getYaw() # rotation matrix from body (local) to inertial (global) nRb = math.Matrix2.nRb(yaw) newAccel = nRb * newAccel newVelocity = nRb * newVelocity newPosition = self._initialGlobalPosition + (nRb * newPosition) # send the new values to the controller #if currentTime <= self._trajectory.getFinalTime(): if newAccel is None: if newVelocity is None: self._controller.translate(newPosition) else: self._controller.translate(newPosition, newVelocity) else: self._controller.translate(newPosition, newVelocity, newAccel) if (currentTime >= self._trajectory.getFinalTime() and \ self._controller.atPosition() and \ self._controller.atVelocity()): self._finish()
def _update(self, event): # figure out at what time to evaluate the trajectory if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newPosition = self._trajectory.computeValue(currentTime) newVelocity = self._trajectory.computeDerivative(currentTime, 1) newAccel = self._trajectory.computeDerivative(currentTime, 2) if self._frame is Frame.LOCAL: # interpret the trajectory output as being in the local coordinate orientation = self._controller.getDesiredOrientation() yaw = orientation.getYaw() # rotation matrix from body (local) to inertial (global) nRb = math.Matrix2.nRb(yaw) newAccel = nRb * newAccel newVelocity = nRb * newVelocity newPosition = self._initialGlobalPosition + (nRb * newPosition) # send the new values to the controller #if currentTime <= self._trajectory.getFinalTime(): if newAccel is None: if newVelocity is None: self._controller.translate(newPosition) else: self._controller.translate(newPosition, newVelocity) else: self._controller.translate(newPosition, newVelocity, newAccel) if (currentTime >= self._trajectory.getFinalTime() and \ self._controller.atPosition() and \ self._controller.atVelocity()): self._finish()
def _update(self, event): if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newDepth = self._trajectory.computeValue(currentTime) newDepthRate = self._trajectory.computeDerivative(currentTime,1) newDepthAccel = self._trajectory.computeDerivative(currentTime,2) # send the new values to the controller if newDepthAccel is None: if newDepthRate is None: self._controller.changeDepth(newDepth) else: self._controller.changeDepth(newDepth, newDepthRate) else: self._controller.changeDepth(newDepth, newDepthRate, newDepthAccel) if currentTime >= self._trajectory.getFinalTime() and \ self._controller.atDepth(): self._finish()
def _update(self, event): if self._trajectory.isRelative(): currentTime = timer.time() - self._startTime else: currentTime = timer.time() # evaluate the trajectory value and 1st derivative newDepth = self._trajectory.computeValue(currentTime) newDepthRate = self._trajectory.computeDerivative(currentTime, 1) newDepthAccel = self._trajectory.computeDerivative(currentTime, 2) # send the new values to the controller if newDepthAccel is None: if newDepthRate is None: self._controller.changeDepth(newDepth) else: self._controller.changeDepth(newDepth, newDepthRate) else: self._controller.changeDepth(newDepth, newDepthRate, newDepthAccel) if currentTime >= self._trajectory.getFinalTime() and \ self._controller.atDepth(): self._finish()
def recordClipImpl(addRecorder, removeRecorder, seconds, name, extension, rate): # All comments are needed. Do not add blank lines! TIMEOUT = core.declareEventType('TIMEOUT') conn = None def stop(event): removeRecorder(name + extension) conn.disconnect() conn = queuedEventHub.subscribeToType(TIMEOUT, stop) # If no name was given, create one out of the time if name is None: timeStamp = datetime.fromtimestamp(timer.time()) name = timeStamp.strftime("%Y%m%d%H%M%S") # Add the forward recorder addRecorder(name + extension, rate) # Create the timer clipTimer = timerManager.newTimer(TIMEOUT, seconds) clipTimer.start()
def _seek(self): """ Commands the controller to seek the current vehicle @note: Everything is in DEGREES! """ # Compute time since the last run deltaT = 1.0 / 40.0 now = timer.time() if self._lastSeekTime != 0.0: deltaT = now - self._lastSeekTime self._lastSeekTime = now # Determine new Depth if 0 != self._depthGain: dtDepth, self._sumDepth, self._oldDepth = common.PIDLoop( x=self._target.y, xd=0, dt=deltaT, dtTooSmall=1.0 / 100.0, dtTooBig=1.0, kp=self._depthGain, kd=self._dDepthGain, ki=self._iDepthGain, sum=self._sumDepth, xOld=self._oldDepth) # Clamp depth change if self._maxDepthDt != 0: if dtDepth > self._maxDepthDt: dtDepth = self._maxDepthDt elif dtDepth < -self._maxDepthDt: dtDepth = -self._maxDepthDt currentDepth = self._estimator.getEstimatedDepth() newDepth = currentDepth + dtDepth self._controller.changeDepth(newDepth) # Do pointing control if not self._translate: # Determine how to yaw the vehicle vehicleHeading = self._estimator.getEstimatedOrientation().getYaw( True) vehicleHeading = vehicleHeading.valueDegrees() absoluteTargetHeading = vehicleHeading + self._target.azimuth desiredHeading = self._controller.getDesiredOrientation().getYaw( True) desiredHeading = desiredHeading.valueDegrees() yawCommand = (absoluteTargetHeading - desiredHeading) * self._yawGain if self._maxYaw is not None and abs(yawCommand) > self._maxYaw: yawCommand = self._maxYaw * (yawCommand / abs(yawCommand)) self._controller.yawVehicle(yawCommand, 0) else: sidewaysSpeed, self._sumX, self._oldX = common.PIDLoop( x=self._target.x, xd=0, dt=deltaT, dtTooSmall=1.0 / 100.0, dtTooBig=1.0, kp=self._translateGain, kd=self._dTranslateGain, ki=self._iTranslateGain, sum=self._sumX, xOld=self._oldX) self._controller.setSidewaysSpeed(-1 * sidewaysSpeed) # Drive toward light if self._maxSpeed != 0: self._controller.translate(ext.math.Vector2(1, 0), ext.math.Vector2(0.1, 0)) if self._alignment() <= self._alignmentThreshold and not self._first: self.publish(SeekPoint.POINT_ALIGNED, ext.core.Event()) self._first = False
def testNanosleep(self): startTime = timer.time() timer.nanosleep(0, long(0.25 * 1e9)) endTime = timer.time() self.assertAlmostEquals(0.25, endTime - startTime, 1)
def handleTimer(self, event): self.event = event self.endTime = timer.time()
def _seek(self): """ Commands the controller to seek the current vehicle @note: Everything is in DEGREES! """ # Compute time since the last run deltaT = 1.0 / 40.0 now = timer.time() if self._lastSeekTime != 0.0: deltaT = now - self._lastSeekTime self._lastSeekTime = now # Determine new Depth if 0 != self._depthGain: dtDepth, self._sumDepth, self._oldDepth = common.PIDLoop( x=self._target.y, xd=0, dt=deltaT, dtTooSmall=1.0 / 100.0, dtTooBig=1.0, kp=self._depthGain, kd=self._dDepthGain, ki=self._iDepthGain, sum=self._sumDepth, xOld=self._oldDepth, ) # Clamp depth change if self._maxDepthDt != 0: if dtDepth > self._maxDepthDt: dtDepth = self._maxDepthDt elif dtDepth < -self._maxDepthDt: dtDepth = -self._maxDepthDt currentDepth = self._vehicle.getDepth() newDepth = currentDepth + dtDepth self._controller.setDepth(newDepth) # Do pointing control if not self._translate: # Determine how to yaw the vehicle vehicleHeading = self._vehicle.getOrientation().getYaw(True) vehicleHeading = vehicleHeading.valueDegrees() absoluteTargetHeading = vehicleHeading + self._target.azimuth desiredHeading = self._controller.getDesiredOrientation().getYaw(True) desiredHeading = desiredHeading.valueDegrees() yawCommand = (absoluteTargetHeading - desiredHeading) * self._yawGain if self._maxYaw is not None and abs(yawCommand) > self._maxYaw: yawCommand = self._maxYaw * (yawCommand / abs(yawCommand)) self._controller.yawVehicle(yawCommand) else: sidewaysSpeed, self._sumX, self._oldX = common.PIDLoop( x=self._target.x, xd=0, dt=deltaT, dtTooSmall=1.0 / 100.0, dtTooBig=1.0, kp=self._translateGain, kd=self._dTranslateGain, ki=self._iTranslateGain, sum=self._sumX, xOld=self._oldX, ) self._controller.setSidewaysSpeed(-1 * sidewaysSpeed) # Drive toward light if self._maxSpeed != 0: self._controller.setSpeed(self._speedScale() * self._maxSpeed) if self._alignment() <= self._alignmentThreshold and not self._first: self.publish(SeekPoint.POINT_ALIGNED, ext.core.Event()) self._first = False