def __init__(self, planPlayback, playbackRobotModel,
playbackJointController, robotStateModel,
robotStateJointController, manipPlanner):
self.planPlayback = planPlayback
self.playbackRobotModel = playbackRobotModel
self.playbackJointController = playbackJointController
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.manipPlanner = manipPlanner
manipPlanner.connectPlanCommitted(self.onPlanCommitted)
manipPlanner.connectUseSupports(self.updateButtonColor)
self.autoPlay = True
self.useOperationColors()
self.planFramesObj = None
self.plan = None
self.poseInterpolator = None
self.startTime = 0.0
self.endTime = 1.0
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = self.updateAnimation
self.animationClock = SimpleTimer()
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddPlaybackPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
self.ui = WidgetDict(self.widget.children())
self.ui.viewModeCombo.connect('currentIndexChanged(const QString&)',
self.viewModeChanged)
self.ui.playbackSpeedCombo.connect(
'currentIndexChanged(const QString&)', self.playbackSpeedChanged)
self.ui.interpolationCombo.connect(
'currentIndexChanged(const QString&)', self.interpolationChanged)
self.ui.samplesSpinBox.connect('valueChanged(int)',
self.numberOfSamplesChanged)
self.ui.playbackSlider.connect('valueChanged(int)',
self.playbackSliderValueChanged)
self.ui.animateButton.connect('clicked()', self.animateClicked)
self.ui.hideButton.connect('clicked()', self.hideClicked)
self.ui.executeButton.connect('clicked()', self.executeClicked)
self.ui.executeButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
self.ui.stopButton.connect('clicked()', self.stopClicked)
self.ui.executeButton.setContextMenuPolicy(QtCore.Qt.CustomContextMenu)
self.ui.executeButton.connect(
'customContextMenuRequested(const QPoint&)',
self.showExecuteContextMenu)
self.setPlan(None)
self.hideClicked()
def playPoses(self, poseTimes, poses, jointController):
f = self.getPoseInterpolator(poseTimes, poses)
timer = SimpleTimer()
def updateAnimation():
tNow = timer.elapsed() * self.playbackSpeed
if tNow > poseTimes[-1]:
pose = poses[-1]
jointController.setPose('plan_playback', pose)
if self.animationCallback:
self.animationCallback()
return False
pose = f(tNow)
jointController.setPose('plan_playback', pose)
if self.animationCallback:
self.animationCallback()
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
updateAnimation()
def waitForResult(self, timeout=None):
t = SimpleTimer()
while self.isAlive():
result = self.checkForResult()
if result is not None:
return result
if timeout is not None and t.elapsed() > timeout:
return None
def run(self):
delayTime = self.properties.getProperty('Delay time')
t = SimpleTimer()
while True:
elapsed = t.elapsed()
if elapsed >= delayTime:
break
self.statusMessage = 'Waiting %.1f seconds' % (delayTime - elapsed)
yield
def __init__(self):
self.lastAtlasStatusMessage = None
self.lastControllerStatusMessage = None
self.lastAtlasBatteryDataMessage = None
self.lastAtlasElectricArmStatusMessage = None
self.lastControllerRateMessage = None
self.maxPressureHistory = deque([0.0], 10)
self.averageRecentMaxPressure = 0.0
self._setupSubscriptions()
self.timer = SimpleTimer()
self.sentStandUtime = None
self.startupStage = 0
self._behaviorMap = None
self._controllerStatusMap = None
def run(self):
def getMsg():
return robotSystem.atlasDriver.lastControllerStatusMessage
def isExecuting():
return getMsg(
).execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING
# wait for first status message
while not getMsg():
yield
if isExecuting():
raise Exception(
'error, invoked during plan execution and cannot guarantee safety.'
)
t = SimpleTimer()
lastPlanStartTime = getMsg().last_plan_start_utime
# wait for next plan to begin
self.statusMessage = 'Waiting for %s to begin...' % self.getTypeLabel()
while getMsg().last_plan_start_utime == lastPlanStartTime:
if t.elapsed() > self.properties.getProperty('Timeout'):
yield self.promptUserForPlanRecommit()
t.reset()
self.recommitPlan()
else:
yield
# wait for execution
self.statusMessage = 'Waiting for %s execution...' % self.getTypeLabel(
)
while getMsg(
).execution_status == lcmdrc.plan_status_t.EXECUTION_STATUS_EXECUTING:
if getMsg().plan_type != self.getType():
raise Exception('error, unexpected execution plan type: %s' %
getMsg().plan_type)
yield
self.statusMessage = 'Waiting for recent robot state...'
while robotSystem.robotStateJointController.lastRobotStateMessage.utime < getMsg(
).last_plan_start_utime:
yield
def updateState(self):
t = SimpleTimer()
self.manager.updateExistingLoggerProcesses()
activeLogFiles = self.manager.getActiveLogFilenames()
self.numProcesses = len(self.manager.getActiveLoggerPids())
self.numLogFiles = len(activeLogFiles)
if self.numLogFiles == 1:
self.lastActiveLogFile = activeLogFiles[0]
if self.numProcesses == 0:
self.button.text = 'start logger'
elif self.numProcesses == 1:
self.button.text = 'stop logger'
elif self.numProcesses > 1:
self.button.text = 'stop all loggers'
statusDescription = 'active' if self.numProcesses else 'last'
logFileDescription = self.lastActiveLogFile or '<unknown>'
self.button.setToolTip('%s log file: %s' %
(statusDescription, logFileDescription))
def delay(self, delayTimeInSeconds):
yield
t = SimpleTimer()
while t.elapsed() < delayTimeInSeconds:
yield
def testJoint(self, jointId):
self.jointId = jointId
self.testTimer = SimpleTimer()
self.start()
def __init__(self):
self.subscriber = None
self.transforms = []
self.timer = SimpleTimer()
self.paused = True
def __call__(self):
t = SimpleTimer()
while t.elapsed() < self.delayTimeInSeconds:
yield
