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 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 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 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 testNanosleep(self):
startTime = timer.time()
timer.nanosleep(0, long(0.25 * 1e9))
endTime = timer.time()
self.assertAlmostEquals(0.25, endTime - startTime, 1)
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
def handleTimer(self, event):
self.event = event
self.endTime = timer.time()
