def __init__(self):
self.timer = TimerCallback(targetFps=10)
#self.timer.disableScheduledTimer()
self.app = ConsoleApp()
self.robotModel, self.jointController = roboturdf.loadRobotModel(
'robot model')
self.fpsCounter = simpletimer.FPSCounter()
self.drakePoseJointNames = robotstate.getDrakePoseJointNames()
self.fpsCounter.printToConsole = True
self.timer.callback = self._tick
self._initialized = False
self.publishChannel = 'JOINT_POSITION_GOAL'
# self.lastCommandMessage = newAtlasCommandMessageAtZero()
self._numPositions = len(robotstate.getDrakePoseJointNames())
self._previousElapsedTime = 100
self._baseFlag = 0
self.jointLimitsMin = np.array([
self.robotModel.model.getJointLimits(jointName)[0]
for jointName in robotstate.getDrakePoseJointNames()
])
self.jointLimitsMax = np.array([
self.robotModel.model.getJointLimits(jointName)[1]
for jointName in robotstate.getDrakePoseJointNames()
])
self.useControllerFlag = False
self.drivingGainsFlag = False
self.applyDefaults()
def _initTaskPanel(self):
self.lastStatusMessage = ''
self.nextStepTask = None
self.completedTasks = []
self.taskQueue = atq.AsyncTaskQueue()
self.taskQueue.connectQueueStarted(self.onQueueStarted)
self.taskQueue.connectQueueStopped(self.onQueueStopped)
self.taskQueue.connectTaskStarted(self.onTaskStarted)
self.taskQueue.connectTaskEnded(self.onTaskEnded)
self.taskQueue.connectTaskPaused(self.onTaskPaused)
self.taskQueue.connectTaskFailed(self.onTaskFailed)
self.taskQueue.connectTaskException(self.onTaskException)
self.timer = TimerCallback(targetFps=2)
self.timer.callback = self.updateTaskStatus
self.timer.start()
self.taskTree = tmw.TaskTree()
self.ui.taskFrame.layout().insertWidget(0, self.taskTree.treeWidget)
l = QtGui.QVBoxLayout(self.ui.taskPropertiesGroupBox)
l.addWidget(self.taskTree.propertiesPanel)
PythonQt.dd.ddGroupBoxHider(self.ui.taskPropertiesGroupBox)
self.ui.taskStepButton.connect('clicked()', self.onStep)
self.ui.taskContinueButton.connect('clicked()', self.onContinue)
self.ui.taskPauseButton.connect('clicked()', self.onPause)
self.ui.promptAcceptButton.connect('clicked()', self.onAcceptPrompt)
self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt)
self.clearPrompt()
self.updateTaskButtons()
class LCMForceDisplay(object):
'''
Displays foot force sensor signals in a status bar widget or label widget
'''
def onAtlasState(self,msg):
self.l_foot_force_z = msg.force_torque.l_foot_force_z
self.r_foot_force_z = msg.force_torque.r_foot_force_z
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmdrc.atlas_state_t, self.onAtlasState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def __del__(self):
lcmUtils.removeSubscriber(self.sub)
def showRate(self):
global leftInContact, rightInContact
self.label.text = '%.2f | %.2f' % (self.l_foot_force_z,self.r_foot_force_z)
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection('MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection('AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel,
self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)',
self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def main():
app = consoleapp.ConsoleApp()
meshCollection = lcmobjectcollection.LCMObjectCollection(
'MESH_COLLECTION_COMMAND')
affordanceCollection = lcmobjectcollection.LCMObjectCollection(
'AFFORDANCE_COLLECTION_COMMAND')
meshCollection.sendEchoRequest()
affordanceCollection.sendEchoRequest()
def printCollection():
print
print '----------------------------------------------------'
print datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
print '%d affordances' % len(affordanceCollection.collection)
for desc in affordanceCollection.collection.values():
print
print 'name:', desc['Name']
print 'type:', desc['classname']
timer = TimerCallback(targetFps=0.2)
timer.callback = printCollection
timer.start()
#app.showPythonConsole()
app.start()
def __init__(self, frameProvider):
vis.PolyDataItem.__init__(self, 'spindle axis', vtk.vtkPolyData(), view=None)
self.frameProvider = frameProvider
self.timer = TimerCallback()
self.timer.callback = self.update
self.setProperty('Color', QtGui.QColor(0, 255, 0))
self.setProperty('Visible', False)
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def __init__(self, view):
TimerCallback.__init__(self)
self.view = view
self.flyTime = 0.5
self.startTime = 0.0
self.maintainViewDirection = False
self.positionZoom = 0.7
def __init__(self, view, affordanceManager, ikServer, jointController, raycastDriver):
self.view = view
self.affordanceManager = affordanceManager
self.ikServer = ikServer
self.jointController = jointController
self.raycastDriver = raycastDriver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddAffordancePanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.affordanceListWidget.hide()
self.ui.spawnBoxButton.connect('clicked()', self.onSpawnBox)
self.ui.spawnSphereButton.connect('clicked()', self.onSpawnSphere)
self.ui.spawnCylinderButton.connect('clicked()', self.onSpawnCylinder)
self.ui.spawnCapsuleButton.connect('clicked()', self.onSpawnCapsule)
self.ui.spawnRingButton.connect('clicked()', self.onSpawnRing)
self.ui.spawnMeshButton.connect('clicked()', self.onSpawnMesh)
self.ui.getRaycastTerrainButton.connect('clicked()', self.onGetRaycastTerrain)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
self.updateTimer = TimerCallback(targetFps=30)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
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 startApplication(enableQuitTimer=False):
appInstance = QtGui.QApplication.instance()
if enableQuitTimer:
quitTimer = TimerCallback()
quitTimer.callback = appInstance.quit
quitTimer.singleShot(0.1)
appInstance.exec_()
def startApplication(enableQuitTimer=False):
appInstance = QtGui.QApplication.instance()
if enableQuitTimer:
quitTimer = TimerCallback()
quitTimer.callback = appInstance.quit
quitTimer.singleShot(0.1)
appInstance.exec_()
def showHandCloud(hand='left', view=None):
view = view or app.getCurrentRenderView()
if view is None:
return
assert hand in ('left', 'right')
maps = om.findObjectByName('Map Server')
assert maps is not None
viewId = 52 if hand == 'left' else 53
reader = maps.source.reader
def getCurrentViewId():
return reader.GetCurrentMapId(viewId)
p = vtk.vtkPolyData()
obj = showPolyData(p, '%s hand cloud' % hand, view=view, parent='sensors')
obj.currentViewId = -1
def updateCloud():
currentViewId = getCurrentViewId()
#print 'updateCloud: current view id:', currentViewId
if currentViewId != obj.currentViewId:
reader.GetDataForMapId(viewId, currentViewId, p)
#print 'updated poly data. %d points.' % p.GetNumberOfPoints()
obj._renderAllViews()
t = TimerCallback()
t.targetFps = 1
t.callback = updateCloud
t.start()
obj.updater = t
return obj
def __init__(self, robotStateJointController, view, cameraview,
mapServerSource):
self.robotStateJointController = robotStateJointController
self.view = view
self.cameraview = cameraview
self.mapServerSource = mapServerSource
self.lastCameraMessageTime = 0
self.lastScanBundleMessageTime = 0
self.lastBlackoutLengths = []
self.lastBlackoutLength = 0
self.inBlackout = False
self.averageBlackoutLength = 0.0
self.txt = vis.updateText("DATA AGE: 0 sec",
"Data Age Text",
view=self.view)
self.txt.addProperty('Show Avg Duration', False)
self.txt.setProperty('Visible', False)
self.updateTimer = TimerCallback(self.UPDATE_RATE)
self.updateTimer.callback = self.update
self.updateTimer.start()
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 onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
jointController = self.robotSystem.teleopJointController
timer = simpletimer.SimpleTimer()
def setPose(pose):
jointController.setPose('plan_playback', pose)
self.jointTeleopPanel.endPose = pose
self.jointTeleopPanel.updateSliders()
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def start(self):
if self.reader is None:
self.reader = drc.vtkMultisenseSource()
self.reader.InitBotConfig(drcargs.args().config_file)
self.reader.SetDistanceRange(0.25, 4.0)
self.reader.Start()
TimerCallback.start(self)
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
class BlackoutMonitor(object):
UPDATE_RATE = 5
AVERAGE_N = 5
def __init__(self, robotStateJointController, view, cameraview, mapServerSource):
self.robotStateJointController = robotStateJointController
self.view = view
self.cameraview = cameraview
self.mapServerSource = mapServerSource
self.lastCameraMessageTime = 0
self.lastScanBundleMessageTime = 0
self.lastBlackoutLengths = []
self.lastBlackoutLength = 0
self.inBlackout = False
self.averageBlackoutLength = 0.0
self.txt = vis.updateText("DATA AGE: 0 sec", "Data Age Text", view=self.view)
self.txt.addProperty('Show Avg Duration', False)
self.txt.setProperty('Visible', False)
self.updateTimer = TimerCallback(self.UPDATE_RATE)
self.updateTimer.callback = self.update
self.updateTimer.start()
def update(self):
self.lastCameraMessageTime = self.cameraview.imageManager.queue.getCurrentImageTime('CAMERA_LEFT')
self.lastScanBundleMessageTime = self.mapServerSource.reader.GetLastScanBundleUTime()
if self.robotStateJointController.lastRobotStateMessage:
elapsedCam = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastCameraMessageTime) / (1000*1000), 0.0)
elapsedScan = max((self.robotStateJointController.lastRobotStateMessage.utime - self.lastScanBundleMessageTime) / (1000*1000), 0.0)
# can't be deleted, only hidden, so this is ok
if (self.txt.getProperty('Visible')):
if (self.txt.getProperty('Show Avg Duration')):
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec AVG: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan), math.floor(self.averageBlackoutLength))
else:
textstr = "CAM AGE: %02d sec\nSCAN AGE: %02d sec" % (math.floor(elapsedCam), math.floor(elapsedScan))
ssize = self.view.size
self.txt.setProperty('Text', textstr)
self.txt.setProperty('Position', [10, 10])
# count out blackouts
if elapsedCam > 1.0:
self.inBlackout = True
self.lastBlackoutLength = elapsedCam
else:
if (self.inBlackout):
# Don't count huge time jumps due to init
if (self.lastBlackoutLength < 100000):
self.lastBlackoutLengths.append(self.lastBlackoutLength)
if len(self.lastBlackoutLengths) > self.AVERAGE_N:
self.lastBlackoutLengths.pop(0)
if len(self.lastBlackoutLengths) > 0:
self.averageBlackoutLength = sum(self.lastBlackoutLengths) / float(len(self.lastBlackoutLengths))
self.inBlackout = False
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
def __init__(self, view, callbackFunc=None):
TimerCallback.__init__(self)
self.reader = None
self.folder = None
self.view = view
self.displayedMapIds = {}
self.polyDataObjects = {}
self.targetFps = 10
self.callbackFunc = callbackFunc
self.colorizeCallback = None
self.useMeshes = True
def __init__(self, view):
self.collection = lcmobjectcollection.LCMObjectCollection(
channel='AFFORDANCE_COLLECTION_COMMAND')
self.collection.connectDescriptionUpdated(self._onDescriptionUpdated)
self.collection.connectDescriptionRemoved(self._onDescriptionRemoved)
self.view = view
self.notifyFrequency = 30 # throttle lcm messages per second sent for affordance updates
self._ignoreChanges = False
self._pendingUpdates = set()
self.timer = TimerCallback()
self.timer.callback = self._notifyPendingUpdates
def __init__(self, channel, statusBar=None):
self.sub = lcmUtils.addSubscriber(channel, lcmdrc.atlas_state_t, self.onAtlasState)
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.showRate
self.timer.start()
self.l_foot_force_z = 0
self.r_foot_force_z = 0
def __init__(self, driver):
self.driver = driver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddAtlasDriverPanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.widget.setWindowTitle('Atlas Driver Panel')
self.ui = WidgetDict(self.widget.children())
# Main Panel
self.ui.calibrateEncodersButton.connect('clicked()',
self.onCalibrateEncoders)
self.ui.prepButton.connect('clicked()', self.onPrep)
self.ui.combinedStandButton.connect('clicked()', self.onCombinedStand)
self.ui.stopButton.connect('clicked()', self.onStop)
self.ui.freezeButton.connect('clicked()', self.onFreeze)
self.ui.calibrateNullBiasButton.connect('clicked()',
self.onCalibrateNullBias)
self.ui.calibrateElectricArmsButton.connect(
'clicked()', self.onCalibrateElectricArms)
self.ui.initNavButton.connect('clicked()', self.onInitNav)
self.ui.standButton.connect('clicked()', self.onStand)
self.ui.mitStandButton.connect('clicked()', self.onMITStand)
self.ui.userButton.connect('clicked()', self.onUser)
self.ui.manipButton.connect('clicked()', self.onManip)
self.ui.recoveryOnButton.connect('clicked()',
self.driver.sendRecoveryEnable)
self.ui.recoveryOffButton.connect('clicked()',
self.driver.sendRecoveryDisable)
self.ui.bracingOnButton.connect('clicked()',
self.driver.sendBracingEnable)
self.ui.bracingOffButton.connect('clicked()',
self.driver.sendBracingDisable)
for psi in [1000, 1500, 2000, 2400, 2650]:
self.ui.__dict__['setPressure' + str(psi) + 'Button'].connect(
'clicked()', partial(self.driver.sendDesiredPumpPsi, psi))
self.ui.sendCustomPressureButton.connect('clicked()',
self.sendCustomPressure)
self.setupElectricArmCheckBoxes()
PythonQt.dd.ddGroupBoxHider(self.ui.calibrationGroupBox)
PythonQt.dd.ddGroupBoxHider(self.ui.pumpStatusGroupBox)
PythonQt.dd.ddGroupBoxHider(self.ui.electricArmStatusGroupBox)
self.updateTimer = TimerCallback(targetFps=5)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
self.updatePanel()
class CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN',
lcmdrc.robot_plan_t,
self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t,
self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] -
poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def __init__(self):
self.tasks = []
self.generators = []
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.callbackLoop
self.callbacks = callbacks.CallbackRegistry([self.QUEUE_STARTED_SIGNAL,
self.QUEUE_STOPPED_SIGNAL,
self.TASK_STARTED_SIGNAL,
self.TASK_ENDED_SIGNAL,
self.TASK_PAUSED_SIGNAL,
self.TASK_FAILED_SIGNAL,
self.TASK_EXCEPTION_SIGNAL])
self.currentTask = None
self.isRunning = False
class TriggerFingerPublisher(): def __init__(self, lcmChannel): self.lcmChannel = lcmChannel self.reader = midi.MidiReader() self.timer = TimerCallback() self.timer.callback = self.tick self.msg = lcmdrc.trigger_finger_t() def startPublishing(self): print 'Publishing on ' + self.lcmChannel self.timer.start() def publish(self): messages = self.reader.getMessages() for message in messages: channel = message[2] val = message[3] / 127.0 if channel is 102: self.msg.slider1 = val elif channel is 103: self.msg.slider2 = val elif channel is 104: self.msg.slider3 = val elif channel is 105: self.msg.slider4 = val elif channel is 106: self.msg.knob1 = val elif channel is 107: self.msg.knob2 = val elif channel is 108: self.msg.knob3 = val elif channel is 109: self.msg.knob4 = val elif channel is 110: self.msg.knob5 = val elif channel is 111: self.msg.knob6 = val elif channel is 112: self.msg.knob7 = val elif channel is 113: self.msg.knob8 = val if len(messages) is not 0: self.msg.utime = getUtime() lcmUtils.publish(self.lcmChannel, self.msg) def tick(self): self.publish()
class TriggerFingerPublisher():
def __init__(self, lcmChannel):
self.lcmChannel = lcmChannel
self.reader = midi.MidiReader()
self.timer = TimerCallback()
self.timer.callback = self.tick
self.msg = lcmdrc.trigger_finger_t()
def startPublishing(self):
print 'Publishing on ' + self.lcmChannel
self.timer.start()
def publish(self):
messages = self.reader.getMessages()
for message in messages:
channel = message[2]
val = message[3] / 127.0
if channel is 102:
self.msg.slider1 = val
elif channel is 103:
self.msg.slider2 = val
elif channel is 104:
self.msg.slider3 = val
elif channel is 105:
self.msg.slider4 = val
elif channel is 106:
self.msg.knob1 = val
elif channel is 107:
self.msg.knob2 = val
elif channel is 108:
self.msg.knob3 = val
elif channel is 109:
self.msg.knob4 = val
elif channel is 110:
self.msg.knob5 = val
elif channel is 111:
self.msg.knob6 = val
elif channel is 112:
self.msg.knob7 = val
elif channel is 113:
self.msg.knob8 = val
if len(messages) is not 0:
self.msg.utime = getUtime()
lcmUtils.publish(self.lcmChannel, self.msg)
def tick(self):
self.publish()
class CommittedRobotPlanListener(object):
def __init__(self):
self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause)
self.animationTimer = None
def onPause(self, msg):
commandStream.stopStreaming()
if self.animationTimer:
self.animationTimer.stop()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
print 'received robot plan, %.2f seconds' % (poseTimes[-1] - poseTimes[0])
commandStream.applyPlanDefaults()
commandStream.startStreaming()
timer = simpletimer.SimpleTimer()
def setPose(pose):
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
commandStream.applyDefaults()
print 'plan ended.'
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 1000
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def _initTaskPanel(self):
self.lastStatusMessage = ''
self.nextStepTask = None
self.completedTasks = []
self.taskQueue = atq.AsyncTaskQueue()
self.taskQueue.connectQueueStarted(self.onQueueStarted)
self.taskQueue.connectQueueStopped(self.onQueueStopped)
self.taskQueue.connectTaskStarted(self.onTaskStarted)
self.taskQueue.connectTaskEnded(self.onTaskEnded)
self.taskQueue.connectTaskPaused(self.onTaskPaused)
self.taskQueue.connectTaskFailed(self.onTaskFailed)
self.taskQueue.connectTaskException(self.onTaskException)
self.timer = TimerCallback(targetFps=2)
self.timer.callback = self.updateTaskStatus
self.timer.start()
self.taskTree = tmw.TaskTree()
self.ui.taskFrame.layout().insertWidget(0, self.taskTree.treeWidget)
l = QtGui.QVBoxLayout(self.ui.taskPropertiesGroupBox)
l.addWidget(self.taskTree.propertiesPanel)
PythonQt.dd.ddGroupBoxHider(self.ui.taskPropertiesGroupBox)
self.ui.taskStepButton.connect('clicked()', self.onStep)
self.ui.taskContinueButton.connect('clicked()', self.onContinue)
self.ui.taskPauseButton.connect('clicked()', self.onPause)
self.ui.promptAcceptButton.connect('clicked()', self.onAcceptPrompt)
self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt)
self.clearPrompt()
self.updateTaskButtons()
def onRobotPlan(self, msg):
playback = planplayback.PlanPlayback()
playback.interpolationMethod = 'pchip'
poseTimes, poses = playback.getPlanPoses(msg)
f = playback.getPoseInterpolator(poseTimes, poses)
jointController = self.robotSystem.teleopJointController
timer = simpletimer.SimpleTimer()
def setPose(pose):
jointController.setPose('plan_playback', pose)
self.jointTeleopPanel.endPose = pose
self.jointTeleopPanel.updateSliders()
commandStream.setGoalPose(pose)
def updateAnimation():
tNow = timer.elapsed()
if tNow > poseTimes[-1]:
pose = poses[-1]
setPose(pose)
return False
pose = f(tNow)
setPose(pose)
self.animationTimer = TimerCallback()
self.animationTimer.targetFps = 60
self.animationTimer.callback = updateAnimation
self.animationTimer.start()
def launchViewer(self):
playButtonFps = 1.0/self.options['Sim']['dt']
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
playButton = QtGui.QPushButton('Play/Pause')
playButton.connect('clicked()', self.onPlayButton)
l.addWidget(playButton)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
l.addWidget(panel)
w.showMaximized()
self.carFrame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.carFrame)
applogic.resetCamera(viewDirection=[0.2,0,-1])
self.view.showMaximized()
self.view.raise_()
self.app.start()
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 __init__(self, view, affordanceManager, ikServer, jointController, raycastDriver):
self.view = view
self.affordanceManager = affordanceManager
self.ikServer = ikServer
self.jointController = jointController
self.raycastDriver = raycastDriver
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddAffordancePanel.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.ui.affordanceListWidget.hide()
self.ui.spawnBoxButton.connect('clicked()', self.onSpawnBox)
self.ui.spawnSphereButton.connect('clicked()', self.onSpawnSphere)
self.ui.spawnCylinderButton.connect('clicked()', self.onSpawnCylinder)
self.ui.spawnCapsuleButton.connect('clicked()', self.onSpawnCapsule)
self.ui.spawnRingButton.connect('clicked()', self.onSpawnRing)
self.ui.spawnMeshButton.connect('clicked()', self.onSpawnMesh)
self.ui.getRaycastTerrainButton.connect('clicked()', self.onGetRaycastTerrain)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
self.updateTimer = TimerCallback(targetFps=30)
self.updateTimer.callback = self.updatePanel
self.updateTimer.start()
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def setupPlayback(self):
self.timer = TimerCallback(targetFps=30)
self.timer.callback = self.tick
playButtonFps = 1.0 / self.dt
print "playButtonFPS", playButtonFps
self.playTimer = TimerCallback(targetFps=playButtonFps)
self.playTimer.callback = self.playTimerCallback
self.sliderMovedByPlayTimer = False
panel = QtGui.QWidget()
l = QtGui.QHBoxLayout(panel)
playButton = QtGui.QPushButton('Play/Pause')
playButton.connect('clicked()', self.onPlayButton)
slider = QtGui.QSlider(QtCore.Qt.Horizontal)
slider.connect('valueChanged(int)', self.onSliderChanged)
self.sliderMax = self.numTimesteps
slider.setMaximum(self.sliderMax)
self.slider = slider
l.addWidget(playButton)
l.addWidget(slider)
w = QtGui.QWidget()
l = QtGui.QVBoxLayout(w)
l.addWidget(self.view)
l.addWidget(panel)
w.showMaximized()
self.frame.connectFrameModified(self.updateDrawIntersection)
self.updateDrawIntersection(self.frame)
applogic.resetCamera(viewDirection=[0.2, 0, -1])
self.view.showMaximized()
self.view.raise_()
panel = screengrabberpanel.ScreenGrabberPanel(self.view)
panel.widget.show()
elapsed = time.time() - self.startSimTime
simRate = self.counter / elapsed
print "Total run time", elapsed
print "Ticks (Hz)", simRate
print "Number of steps taken", self.counter
self.app.start()
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
class SpindleSpinChecker(object):
def __init__(self, spindleMonitor):
self.spindleMonitor = spindleMonitor
self.timer = TimerCallback(targetFps=3)
self.timer.callback = self.update
self.warningButton = None
self.action = None
def update(self):
if abs(self.spindleMonitor.getAverageSpindleVelocity()) < 0.2:
self.notifyUserStatusBar()
else:
self.clearStatusBarWarning()
def start(self):
self.action.checked = True
self.timer.start()
def stop(self):
self.action.checked = False
self.timer.stop()
def setupMenuAction(self):
self.action = app.addMenuAction('Tools', 'Spindle Stuck Warning')
self.action.setCheckable(True)
self.action.checked = self.timer.isActive()
self.action.connect('triggered()', self.onActionChanged)
def onActionChanged(self):
if self.action.checked:
self.start()
else:
self.stop()
def clearStatusBarWarning(self):
if self.warningButton:
self.warningButton.deleteLater()
self.warningButton = None
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class ImageWidget(object):
def __init__(self, imageManager, imageName, view):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
rep = self.imageWidget.GetRepresentation()
rep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
rep.SetImage(self.flip.GetOutput())
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def hide(self):
self.imageWidget.Off()
def show(self):
self.imageWidget.On()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
if not self.initialized:
self.show()
self.initialized = True
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source',
shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime,
cameraToLocalFused)
p = filterUtils.transformPolyData(p, cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
def __init__(self, lDriver, rDriver, robotStateModel, robotStateJointController, view):
self.robotStateModel = robotStateModel
self.robotStateJointController = robotStateJointController
self.drivers = {}
self.drivers['left'] = lDriver
self.drivers['right'] = rDriver
self.storedCommand = {'left': None, 'right': None}
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddHandControl.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self._updateBlocked = True
self.widget.advanced.sendButton.setEnabled(True)
# Store calibration for wrist f/t sensors
self.wristftvis = WristForceTorqueVisualizer(robotStateModel, robotStateJointController, view)
# connect the callbacks
self.widget.basic.openButton.clicked.connect(self.openClicked)
self.widget.basic.closeButton.clicked.connect(self.closeClicked)
self.widget.basic.waitOpenButton.clicked.connect(self.waitOpenClicked)
self.widget.basic.waitCloseButton.clicked.connect(self.waitCloseClicked)
self.widget.advanced.sendButton.clicked.connect(self.sendClicked)
self.widget.advanced.calibrateButton.clicked.connect(self.calibrateClicked)
self.widget.advanced.setModeButton.clicked.connect(self.setModeClicked)
self.widget.advanced.regraspButton.clicked.connect(self.regraspClicked)
self.widget.advanced.dropButton.clicked.connect(self.dropClicked)
self.widget.advanced.repeatRateSpinner.valueChanged.connect(self.rateSpinnerChanged)
self.ui.fingerControlButton.clicked.connect(self.fingerControlButton)
self.widget.sensors.rightTareButton.clicked.connect(self.wristftvis.tareRightFT)
self.widget.sensors.leftTareButton.clicked.connect(self.wristftvis.tareLeftFT)
self.widget.sensors.rightCalibButton.clicked.connect(self.wristftvis.calibRightFT)
self.widget.sensors.leftCalibButton.clicked.connect(self.wristftvis.calibLeftFT)
self.widget.sensors.rightCalibClearButton.clicked.connect(self.wristftvis.calibRightClearFT)
self.widget.sensors.leftCalibClearButton.clicked.connect(self.wristftvis.calibLeftClearFT)
self.widget.sensors.rightVisCheck.clicked.connect(self.updateWristFTVis)
self.widget.sensors.leftVisCheck.clicked.connect(self.updateWristFTVis)
self.widget.sensors.torqueVisCheck.clicked.connect(self.updateWristFTVis)
PythonQt.dd.ddGroupBoxHider(self.ui.sensors)
PythonQt.dd.ddGroupBoxHider(self.ui.fingerControl)
# Bug fix... for some reason one slider is set as disabled
self.ui.fingerASlider.setEnabled(True)
# create a timer to repeat commands
self.updateTimer = TimerCallback()
self.updateTimer.callback = self.updatePanel
self.updateTimer.targetFps = 3
self.updateTimer.start()
class ImageWidget(object):
def __init__(self, imageManager, imageName, view):
self.view = view
self.imageManager = imageManager
self.imageName = imageName
self.updateUtime = 0
self.initialized = False
self.imageWidget = vtk.vtkLogoWidget()
rep = self.imageWidget.GetRepresentation()
rep.GetImageProperty().SetOpacity(1.0)
self.imageWidget.SetInteractor(
self.view.renderWindow().GetInteractor())
self.flip = vtk.vtkImageFlip()
self.flip.SetFilteredAxis(1)
self.flip.SetInput(imageManager.getImage(imageName))
rep.SetImage(self.flip.GetOutput())
self.timerCallback = TimerCallback()
self.timerCallback.targetFps = 60
self.timerCallback.callback = self.updateView
self.timerCallback.start()
def hide(self):
self.imageWidget.Off()
def show(self):
self.imageWidget.On()
def updateView(self):
if not self.view.isVisible():
return
currentUtime = self.imageManager.updateImage(self.imageName)
if currentUtime != self.updateUtime:
if not self.initialized:
self.show()
self.initialized = True
self.updateUtime = currentUtime
self.flip.Update()
self.view.render()
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
'''
def __init__(self, atlasDriver, statusBar):
self.atlasDriver = atlasDriver
self.label = QtGui.QLabel('')
statusBar.addPermanentWidget(self.label)
self.timer = TimerCallback(targetFps=1)
self.timer.callback = self.showRate
self.timer.start()
def showRate(self):
rate = self.atlasDriver.getControllerRate()
rate = 'unknown' if rate is None else '%d hz' % rate
self.label.text = 'Controller rate: %s' % rate
class KinectSource(TimerCallback):
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
#self.timerCallback.start()
def start(self):
self.timerCallback.start()
def stop(self):
self.timerCallback.stop()
def setFPS(self, framerate):
self.targetFps = framerate
self.timerCallback.stop()
self.timerCallback.targetFps = framerate
self.timerCallback.start()
def setVisible(self, visible):
self.polyDataObj.setProperty('Visible', visible)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p,cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
class AffordanceTextureUpdater(object):
def __init__(self, affordanceManager):
self.affordanceManager = affordanceManager
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.updateTextures
self.timer.start()
def updateTexture(self, obj):
if obj.getProperty('Camera Texture Enabled'):
cameraview.applyCameraTexture(obj, cameraview.imageManager)
else:
cameraview.disableCameraTexture(obj)
obj._renderAllViews()
def updateTextures(self):
for aff in affordanceManager.getAffordances():
self.updateTexture(aff)
def __init__(self, affordanceManager, imageView):
self.affordanceManager = affordanceManager
self.prependImageName = False
self.projectFootsteps = False
self.projectAffordances = True
self.extraObjects = []
self.imageView = imageView
self.imageQueue = imageView.imageManager.queue
self.timer = TimerCallback(targetFps=10)
self.timer.callback = self.update
class FrameVisualizationPanel(object):
def __init__(self, view):
self.view = view
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui')
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
self.ui = WidgetDict(self.widget.children())
self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget)
robotModel = om.findObjectByName('robot state model')
self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget)
self.eventFilter = PythonQt.dd.ddPythonEventFilter()
self.ui.scrollArea.installEventFilter(self.eventFilter)
self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize)
self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent)
PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup)
PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup)
self.updateTimer = TimerCallback(targetFps=60)
self.updateTimer.callback = self.updateFrames
self.updateTimer.start()
def onEvent(self, obj, event):
minSize = self.ui.scrollArea.widget().minimumSizeHint.width() + self.ui.scrollArea.verticalScrollBar().width
self.ui.scrollArea.setMinimumWidth(minSize)
def updateFrames(self):
self.botFrameUpdater.updateFrames()
def getNameFilter(self):
return str(self.ui.botFramesFilterEdit.text)
def onNameFilterChanged(self):
filter = self.getNameFilter()
def __init__(self, view):
self.collection = lcmobjectcollection.LCMObjectCollection(channel='AFFORDANCE_COLLECTION_COMMAND')
self.collection.connectDescriptionUpdated(self._onDescriptionUpdated)
self.collection.connectDescriptionRemoved(self._onDescriptionRemoved)
self.view = view
self.notifyFrequency = 30 # throttle lcm messages per second sent for affordance updates
self._ignoreChanges = False
self._pendingUpdates = set()
self.timer = TimerCallback()
self.timer.callback = self._notifyPendingUpdates
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 startSwarmVisualization():
global timerCallback, nav_data, nav_cloud
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj = vis.showPolyData(shallowCopy(nav_cloud), 'nav data')
nav_cloud_obj.initialized = False
def updateSwarm():
global nav_cloud
if not nav_cloud_obj.initialized:
nav_cloud_obj.mapper.SetColorModeToMapScalars()
nav_cloud_obj.initialized = True
#print nav_data.shape[0], nav_cloud.GetNumberOfPoints()
nav_cloud = vtknp.getVtkPolyDataFromNumpyPoints(nav_data)
nav_cloud_obj.setPolyData(shallowCopy(nav_cloud))
#print nav_cloud_obj.polyData.GetNumberOfPoints()
timerCallback = TimerCallback(targetFps=30)
timerCallback.callback = updateSwarm
timerCallback.start()
class PointerTracker(object):
'''
See segmentation.estimatePointerTip() documentation.
'''
def __init__(self, robotModel, stereoPointCloudItem):
self.robotModel = robotModel
self.stereoPointCloudItem = stereoPointCloudItem
self.timer = TimerCallback(targetFps=5)
self.timer.callback = self.updateFit
def start(self):
self.timer.start()
def stop(self):
self.timer.stop()
def cleanup(self):
om.removeFromObjectModel(om.findObjectByName('segmentation'))
def updateFit(self, polyData=None):
#if not self.stereoPointCloudItem.getProperty('Visible'):
# return
if not polyData:
self.stereoPointCloudItem.update()
polyData = self.stereoPointCloudItem.polyData
if not polyData or not polyData.GetNumberOfPoints():
self.cleanup()
return
self.tipPosition = segmentation.estimatePointerTip(self.robotModel, polyData)
if self.tipPosition is None:
self.cleanup()
def getPointerTip(self):
return self.tipPosition
