Python TimerCallback.stop方法代码示例

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class AnimateRobot(object):

    def __init__(self, obj, view):
        self.obj = obj
        self.view = view
        self.timer = TimerCallback(targetFps=60)
        self.timer.callback = self.tick

    def start(self):
        self.startTime = time.time()
        self.timer.start()

    def stop(self):
        self.timer.stop()

    def tick(self):
        tNow = time.time()
        elapsed = tNow - self.startTime

        x, y = np.sin(elapsed), np.cos(elapsed)
        angle = -elapsed

        t = vtk.vtkTransform()
        t.PostMultiply()
        t.RotateZ(np.degrees(angle))
        t.Translate(x, y, 0)
        self.obj.getChildFrame().copyFrame(t)
        self.view.render()

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
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

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
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)

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
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

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class SpindleAxisDebug(vis.PolyDataItem):

    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 _onPropertyChanged(self, propertySet, propertyName):
        vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)

        if propertyName == 'Visible':
            if self.getProperty(propertyName):
                self.timer.start()
            else:
                self.timer.stop()

    def onRemoveFromObjectModel(self):
        vis.PolyDataItem.onRemoveFromObjectModel(self)
        self.timer.stop()

    def update(self):

        t = self.frameProvider.getFrame('SCAN')

        p1 = [0.0, 0.0, 0.0]
        p2 = [2.0, 0.0, 0.0]

        p1 = t.TransformPoint(p1)
        p2 = t.TransformPoint(p2)

        d = DebugData()
        d.addSphere(p1, radius=0.01, color=[0,1,0])
        d.addLine(p1, p2, color=[0,1,0])
        self.setPolyData(d.getPolyData())

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class AsyncTaskQueue(object):

    QUEUE_STARTED_SIGNAL = 'QUEUE_STARTED_SIGNAL'
    QUEUE_STOPPED_SIGNAL = 'QUEUE_STOPPED_SIGNAL'
    TASK_STARTED_SIGNAL = 'TASK_STARTED_SIGNAL'
    TASK_ENDED_SIGNAL = 'TASK_ENDED_SIGNAL'
    TASK_PAUSED_SIGNAL = 'TASK_PAUSED_SIGNAL'
    TASK_FAILED_SIGNAL = 'TASK_FAILED_SIGNAL'
    TASK_EXCEPTION_SIGNAL = 'TASK_EXCEPTION_SIGNAL'

    class PauseException(Exception):
        pass

    class FailException(Exception):
        pass

    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

    def reset(self):
        assert not self.isRunning
        assert not self.generators
        self.tasks = []

    def start(self):
        self.isRunning = True
        self.callbacks.process(self.QUEUE_STARTED_SIGNAL, self)
        self.timer.start()

    def stop(self):
        self.isRunning = False
        self.currentTask = None
        self.generators = []
        self.timer.stop()
        self.callbacks.process(self.QUEUE_STOPPED_SIGNAL, self)

    def wrapGenerator(self, generator):
        def generatorWrapper():
            return generator
        return generatorWrapper

    def addTask(self, task):

        if isinstance(task, types.GeneratorType):
            task = self.wrapGenerator(task)

        assert hasattr(task, '__call__')
        self.tasks.append(task)

    def callbackLoop(self):

        try:
            for i in xrange(10):
                self.doWork()
            if not self.tasks:
                self.stop()

        except AsyncTaskQueue.PauseException:
            assert self.currentTask
            self.callbacks.process(self.TASK_PAUSED_SIGNAL, self, self.currentTask)
            self.stop()
        except AsyncTaskQueue.FailException:
            assert self.currentTask
            self.callbacks.process(self.TASK_FAILED_SIGNAL, self, self.currentTask)
            self.stop()
        except:
            assert self.currentTask
            self.callbacks.process(self.TASK_EXCEPTION_SIGNAL, self, self.currentTask)
            self.stop()
            raise

        return self.isRunning

    def popTask(self):
        assert not self.isRunning
        assert not self.currentTask
        if self.tasks:
            self.tasks.pop(0)

    def completePreviousTask(self):
        assert self.currentTask
        self.tasks.remove(self.currentTask)
        self.callbacks.process(self.TASK_ENDED_SIGNAL, self, self.currentTask)
        self.currentTask = None

    def startNextTask(self):
        self.currentTask = self.tasks[0]
        self.callbacks.process(self.TASK_STARTED_SIGNAL, self, self.currentTask)
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class PlaybackPanel(object):

    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 useDevelopmentColors(self):
        self.robotStateModelDisplayAlpha = 0.1
        self.playbackRobotModelUseTextures = True
        self.playbackRobotModelDisplayAlpha = 1

    def useOperationColors(self):
        self.robotStateModelDisplayAlpha = 1
        self.playbackRobotModelUseTextures = False
        self.playbackRobotModelDisplayAlpha = 0.5
    def showExecuteContextMenu(self, clickPosition):

        globalPos = self.ui.executeButton.mapToGlobal(clickPosition)

        menu = QtGui.QMenu()
        menu.addAction('Visualization Only')

        if not self.isPlanFeasible():
            menu.addSeparator()
            if self.isPlanAPlanWithSupports():
                menu.addAction('Execute infeasible plan with supports')
            else:
                menu.addAction('Execute infeasible plan')
        elif self.isPlanAPlanWithSupports():
            menu.addSeparator()
            menu.addAction('Execute plan with supports')

        selectedAction = menu.exec_(globalPos)
        if not selectedAction:
            return

        if selectedAction.text == 'Visualization Only':
            self.executePlan(visOnly=True)
        elif selectedAction.text == 'Execute infeasible plan':
            self.executePlan(overrideInfeasibleCheck=True)
        elif selectedAction.text == 'Execute plan with supports':
            self.executePlan(overrideSupportsCheck=True)
        elif selectedAction.text == 'Execute infeasible plan with supports':
            self.executePlan(overrideInfeasibleCheck=True, overrideSupportsCheck=True)


    def getViewMode(self):
        return str(self.ui.viewModeCombo.currentText)

    def setViewMode(self, mode):
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]

#.........这里部分代码省略.........

        if not self.running_sim:
            self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3])
            #self.ActionSet.drawActionSetFinal()
            self.ActionSet.drawActionSetFull()
            self.drawChosenFunnel()

    def drawChosenFunnel(self):
        velocity_initial_x = self.stateOverTime[self.currentIdx,2]
        velocity_initial_y = self.stateOverTime[self.currentIdx,3]

        variance_x = 2.5 + abs(velocity_initial_x*0.1)
        variance_y = 2.5 + abs(velocity_initial_y*0.1)
        variance_z = 1.5

        x_index = self.actionIndicesOverTime[self.currentIdx,0]
        y_index = self.actionIndicesOverTime[self.currentIdx,1]

        for i in xrange(0,10):
            time = 0.5/10.0*i
            x_center = self.ActionSet.p_x_trajectories[x_index, i]
            y_center = self.ActionSet.p_y_trajectories[y_index, i]
            z_center = 0.0
            World.buildEllipse(i, [x_center,y_center,z_center], variance_x*time, variance_y*time, variance_z*time, alpha=0.3)


    def tick(self):
        #print timer.elapsed
        #simulate(t.elapsed)
        x = np.sin(time.time())
        y = np.cos(time.time())
        self.setRobotFrameState(x,y,0.0)
        if (time.time() - self.playTime) > self.endTime:
            self.playTimer.stop()

    def tick2(self):
        newtime = time.time() - self.playTime
        print time.time() - self.playTime
        x = np.sin(newtime)
        y = np.cos(newtime)
        self.setRobotFrameState(x,y,0.0)

    # just increment the slider, stop the timer if we get to the end
    def playTimerCallback(self):
        self.sliderMovedByPlayTimer = True
        currentIdx = self.slider.value
        nextIdx = currentIdx + 1
        self.slider.setSliderPosition(nextIdx)
        if currentIdx >= self.sliderMax:
            print "reached end of tape, stopping playTime"
            self.playTimer.stop()

    def onXVelocityChanged(self, value):
        self.initial_XVelocity = value
        print "initial x velocity changed to ", value

    def onYVelocityChanged(self, value):
        self.initial_YVelocity = -value
        print "initial y velocity changed to ", -value

    def onSliderChanged(self, value):
        if not self.sliderMovedByPlayTimer:
            self.playTimer.stop()
        numSteps = len(self.stateOverTime)
        idx = int(np.floor(numSteps*(1.0*value/self.sliderMax)))
        idx = min(idx, numSteps-1)

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class FootLockEstimator(object):

    def __init__(self):
        self.footBias = 0.5
        self.timer = TimerCallback(targetFps=1.0)
        self.timer.callback = self.publishCorrection
        self.clear()


    def capture(self):

        self.pelvisFrame = robotStateModel.getLinkFrame('pelvis')
        self.lfootFrame = robotStateModel.getLinkFrame('l_foot')
        self.rfootFrame = robotStateModel.getLinkFrame('r_foot')

    def clear(self):

        self.lfootFrame = None
        self.rfootFrame = None
        self.pelvisFrame = None

    def printCaptured(self):

        print '--------------------'
        print 'l_foot yaw:', self.lfootFrame.GetOrientation()[2]
        print 'r_foot yaw:', self.rfootFrame.GetOrientation()[2]
        print 'pelvis yaw:', self.pelvisFrame.GetOrientation()[2]

    def getPelvisEstimateFromFoot(self, pelvisFrame, footFrame, previousFootFrame):

        pelvisToWorld = pelvisFrame
        footToWorld = footFrame
        oldFootToWorld = previousFootFrame
        worldToFoot = footToWorld.GetLinearInverse()
        pelvisToFoot = concat(pelvisToWorld, worldToFoot)

        return concat(pelvisToFoot, oldFootToWorld)


    def getPelvisEstimate(self):

        p = robotStateModel.getLinkFrame('pelvis')
        lf = robotStateModel.getLinkFrame('l_foot')
        rf = robotStateModel.getLinkFrame('r_foot')

        pelvisLeft = self.getPelvisEstimateFromFoot(p, lf, self.lfootFrame)
        pelvisRight = self.getPelvisEstimateFromFoot(p, rf, self.rfootFrame)

        return transformUtils.frameInterpolate(pelvisLeft, pelvisRight, self.footBias)

    def onRobotStateModelChanged(self, model=None):

        if self.lfootFrame is None:
            return

        newPelvisToWorld = self.getPelvisEstimate()

        q = robotStateJointController.q.copy()
        q[0:3] = newPelvisToWorld.GetPosition()
        q[3:6] = transformUtils.rollPitchYawFromTransform(newPelvisToWorld)
        playbackJointController.setPose('lock_pose', q)

    def initVisualization(self):
        robotStateModel.connectModelChanged(self.onRobotStateModelChanged)
        playbackRobotModel.setProperty('Visible', True)

    def publishCorrection(self, channel='POSE_YAW_LOCK'):

        pelvisToWorld = self.getPelvisEstimate()

        position, quat = transformUtils.poseFromTransform(pelvisToWorld)

        msg = lcmbot.pose_t()
        msg.utime = robotStateJointController.lastRobotStateMessage.utime
        msg.pos = [0.0, 0.0, 0.0]
        msg.orientation = quat.tolist()

        lcmUtils.publish(channel, msg)

    def enable(self):
        self.capture()
        self.timer.start()

    def disable(self):
        self.timer.stop()
        self.clear()

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class AffordanceInCameraUpdater(object):

    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 getOverlayRenderer(self, imageView):

        if not hasattr(imageView, 'overlayRenderer'):
            renWin = imageView.view.renderWindow()
            renWin.SetNumberOfLayers(2)
            ren = vtk.vtkRenderer()
            ren.SetLayer(1)
            ren.SetActiveCamera(imageView.view.camera())
            renWin.AddRenderer(ren)
            imageView.overlayRenderer = ren
        return imageView.overlayRenderer

    def addActorToImageOverlay(self, obj, imageView):

        obj.addToView(imageView.view)
        imageView.view.renderer().RemoveActor(obj.actor)

        renderers = obj.extraViewRenderers.setdefault(imageView.view, [])
        overlayRenderer = self.getOverlayRenderer(imageView)
        if overlayRenderer not in renderers:
            overlayRenderer.AddActor(obj.actor)
            renderers.append(overlayRenderer)

    def getFolderName(self):
        if self.prependImageName:
            return self.imageView.imageName + ' camera overlay'
        else:
            return 'camera overlay'


    def setupObjectInCamera(self, obj):
        imageView = self.imageView
        obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible'))
        self.addActorToImageOverlay(obj, imageView)
        return obj

    def getTransformedName(self, obj):
        if self.prependImageName:
            return 'overlay ' + self.imageView.imageName + ' ' + obj.getProperty('Name')
        else:
            return 'overlay ' + obj.getProperty('Name')


    def getFootsteps(self):
        plan = om.findObjectByName('footstep plan')
        if plan:
            return [child for child in plan.children() if child.getProperty('Name').startswith('step ')]
        else:
            return []

    def getObjectsToUpdate(self):
        objs = []

        if self.projectAffordances:
            objs += self.affordanceManager.getAffordances()
        if self.projectFootsteps:
            objs += self.getFootsteps()
        objs += self.extraObjects
        return objs

    def getObjectInCamera(self, obj):
        overlayObj = om.findObjectByName(self.getTransformedName(obj))
        return overlayObj or self.setupObjectInCamera(obj)

    def cleanUp(self):
        self.timer.stop()
        om.removeFromObjectModel(om.findObjectByName(self.getFolderName()))

    def update(self):
        imageView = self.imageView

        if not imageView.imageInitialized:
            return

        if not imageView.view.isVisible():
            return

        updated = set()

        for obj in self.getObjectsToUpdate():
            cameraObj = self.getObjectInCamera(obj)
            self.updateObjectInCamera(obj, cameraObj)
            updated.add(cameraObj)

        folder = om.findObjectByName(self.getFolderName())
        if folder:
            for child in folder.children():
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class MultisensePanel(object):

    def __init__(self, multisenseDriver, neckDriver):

        self.multisenseDriver = multisenseDriver
        self.neckDriver = neckDriver
        self.neckPitchChanged = False
        self.multisenseChanged = False

        loader = QtUiTools.QUiLoader()
        uifile = QtCore.QFile(':/ui/ddMultisense.ui')
        assert uifile.open(uifile.ReadOnly)

        self.widget = loader.load(uifile)

        self.ui = WidgetDict(self.widget.children())

        self.updateTimer = TimerCallback(targetFps=2)
        self.updateTimer.callback = self.updatePanel
        self.updateTimer.start()

        self.widget.headCamGainSpinner.setEnabled(False)
        self.widget.headCamExposureSpinner.setEnabled(False)

        #connect the callbacks
        self.widget.neckPitchSpinner.valueChanged.connect(self.neckPitchChange)
        self.widget.spinRateSpinner.valueChanged.connect(self.spinRateChange)
        self.widget.scanDurationSpinner.valueChanged.connect(self.scanDurationChange)
        self.widget.headCamFpsSpinner.valueChanged.connect(self.headCamFpsChange)
        self.widget.headCamGainSpinner.valueChanged.connect(self.headCamGainChange)
        self.widget.headCamExposureSpinner.valueChanged.connect(self.headCamExposureChange)
        self.widget.headAutoGainCheck.clicked.connect(self.headCamAutoGainChange)
        self.widget.ledOnCheck.clicked.connect(self.ledOnCheckChange)
        self.widget.ledBrightnessSpinner.valueChanged.connect(self.ledBrightnessChange)

        self.widget.sendButton.clicked.connect(self.sendButtonClicked)

        self.updatePanel()


    def getCameraFps(self):
        return self.widget.headCamFpsSpinner.value

    def getCameraGain(self):
        return self.widget.headCamGainSpinner.value

    def getCameraExposure(self):
        return self.widget.headCamExposureSpinner.value

    def getCameraLedOn(self):
        return self.widget.ledOnCheck.isChecked()

    def getCameraLedBrightness(self):
        return self.widget.ledBrightnessSpinner.value

    def getCameraAutoGain(self):
        return self.widget.headAutoGainCheck.isChecked()

    def getSpinRate(self):
        return self.widget.spinRateSpinner.value

    def getScanDuration(self):
        return self.widget.scanDurationSpinner.value

    def getNeckPitch(self):
        return self.widget.neckPitchSpinner.value

    def ledBrightnessChange(self, event):
        self.multisenseChanged = True

    def ledOnCheckChange(self, event):
        self.multisenseChanged = True

    def headCamExposureChange(self, event):
        self.multisenseChanged = True

    def headCamAutoGainChange(self, event):
        self.multisenseChanged = True
        self.widget.headCamGainSpinner.setEnabled(not self.getCameraAutoGain())
        self.widget.headCamExposureSpinner.setEnabled(not self.getCameraAutoGain())

    def neckPitchChange(self, event):
        self.neckPitchChanged = True
        self.updateTimer.stop()

    def headCamFpsChange(self, event):
        self.multisenseChanged = True

    def headCamGainChange(self, event):
        self.multisenseChanged = True

    def spinRateChange(self, event):
        self.multisenseChanged = True
        spinRate = self.getSpinRate()

        if spinRate == 0.0:
            scanDuration = 240.0
        else:
            scanDuration = abs(60.0 / (spinRate * 2))
        if scanDuration > 240.0:
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class AtlasCommandStream(object):

    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 initialize(self, currentRobotPose):
        assert not self._initialized
        self._currentCommandedPose = np.array(currentRobotPose)
        self._previousCommandedPose = np.array(currentRobotPose)
        self._goalPose = np.array(currentRobotPose)
        self._initialized = True

    def useController(self):
        self.useControllerFlag = True
        self.publishChannel = 'QP_CONTROLLER_INPUT'

    def setKp(self, Kp, jointName=None):
        if jointName is None:
            self._Kp = Kp*np.ones(self._numPositions)
        else:
            idx = robotstate.getDrakePoseJointNames().index(jointName)
            self._maxSpeed[idx] = Kp

        self.updateKd()

    def setMaxSpeed(self, speed, jointName=None):
        if jointName is None:
            self._maxSpeed = np.deg2rad(speed)*np.ones(self._numPositions)
        else:
            idx = robotstate.getDrakePoseJointNames().index(jointName)
            self._maxSpeed[idx] = np.deg2rad(speed)

    def updateKd(self):
        self._dampingRatio = 1;
        self._Kd = 2*self._dampingRatio*np.sqrt(self._Kp)

    def applyDefaults(self):
        self.setKp(10)
        self.setMaxSpeed(10)

        if self.drivingGainsFlag:
            self.setMaxSpeed(70,'l_arm_lwy')
            self.setKp(50,'l_arm_lwy')
            self.setMaxSpeed(100,'l_leg_aky')
            self.setKp(100,'l_leg_aky')
    
    def applyDrivingDefaults(self):
        self.drivingGainsFlag = True
        self.applyDefaults()

    def applyPlanDefaults(self):
        self.setKp(10)
        self.setMaxSpeed(60)        
        
    def startStreaming(self):
        assert self._initialized
        if not self.timer.isActive():
            self.timer.start()

    def stopStreaming(self):
        self.timer.stop()

    def setGoalPose(self, pose):         
        self._goalPose = self.clipPoseToJointLimits(pose)

    def setIndividualJointGoalPose(self, pose, jointName):
        jointIdx = self.drakePoseJointNames.index(jointName)
        self._goalPose[jointIdx] = pose

    def clipPoseToJointLimits(self,pose):
        pose = np.array(pose)
        pose = np.clip(pose, self.jointLimitsMin, self.jointLimitsMax)
        return pose

    def waitForRobotState(self):
        msg = lcmUtils.captureMessage('EST_ROBOT_STATE', lcmbotcore.robot_state_t)
        pose = robotstate.convertStateMessageToDrakePose(msg)
        pose[:6] = np.zeros(6)
        self.initialize(pose)

    def _updateAndSendCommandMessage(self):
        self._currentCommandedPose = self.clipPoseToJointLimits(self._currentCommandedPose)
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class CameraTrackerManager(object):

    def __init__(self):
        self.target = None
        self.targetFrame = None
        self.trackerClass = None
        self.camera = None
        self.view = None
        self.timer = TimerCallback()
        self.timer.callback = self.updateTimer
        self.addTrackers()
        self.initTracker()

    def updateTimer(self):

        tNow = time.time()
        dt = tNow - self.tLast
        if dt < self.timer.elapsed/2.0:
            return

        self.update()

    def setView(self, view):
        self.view = view
        self.camera = view.camera()

    def setTarget(self, obj):

        if obj == self.target:
            return

        self.disableActiveTracker()

        if not obj:
            return

        self.target = obj
        self.targetFrame = obj.getChildFrame()
        self.callbackId = self.targetFrame.connectFrameModified(self.onTargetFrameModified)

        self.initTracker()

    def disableActiveTracker(self):

        if self.targetFrame:
            self.targetFrame.disconnectFrameModified(self.callbackId)

        self.target = None
        self.targetFrame = None
        self.trackerClass = None
        self.initTracker()

    def update(self):

        tNow = time.time()
        dt = tNow - self.tLast
        self.tLast = tNow

        if self.activeTracker:
            self.activeTracker.dt = dt
            self.activeTracker.update()

    def reset(self):
        self.tLast = time.time()
        if self.activeTracker:
            self.activeTracker.reset()


    def getModeActions(self):
        if self.activeTracker:
            return self.activeTracker.actions
        return []

    def onModeAction(self, actionName):
        if self.activeTracker:
            self.activeTracker.onAction(actionName)

    def getModeProperties(self):
        if self.activeTracker:
            return self.activeTracker.properties
        return None

    def onTargetFrameModified(self, frame):
        self.update()

    def initTracker(self):

        self.timer.stop()
        self.activeTracker = self.trackerClass(self.view, self.targetFrame) if self.trackerClass else None
        self.reset()
        self.update()

        if self.activeTracker:
            minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
            if minimumUpdateRate > 0:
                self.timer.targetFps = minimumUpdateRate
                self.timer.start()

    def addTrackers(self):
        self.trackers = OrderedDict([
#.........这里部分代码省略.........

# 需要导入模块: from director.timercallback import TimerCallback [as 别名]
# 或者: from director.timercallback.TimerCallback import stop [as 别名]
class CameraTrackerManager(object):
    def __init__(self):
        self.target = None
        self.targetFrame = None
        self.trackerClass = None
        self.camera = None
        self.view = None
        self.timer = TimerCallback()
        self.timer.callback = self.updateTimer
        self.addTrackers()
        self.initTracker()

    def updateTimer(self):

        tNow = time.time()
        dt = tNow - self.tLast
        if dt < self.timer.elapsed / 2.0:
            return

        self.update()

    def setView(self, view):
        self.view = view
        self.camera = view.camera()

    def setTarget(self, target):
        """
        target should be an instance of TargetFrameConverter or
        any object that provides a method getTargetFrame().
        """

        if target == self.target:
            return

        self.disableActiveTracker()

        if not target:
            return

        self.target = target
        self.targetFrame = target.getTargetFrame()
        self.callbackId = self.targetFrame.connectFrameModified(self.onTargetFrameModified)

        self.initTracker()

    def disableActiveTracker(self):

        if self.targetFrame:
            self.targetFrame.disconnectFrameModified(self.callbackId)

        self.target = None
        self.targetFrame = None
        self.initTracker()

    def update(self):

        tNow = time.time()
        dt = tNow - self.tLast
        self.tLast = tNow

        if self.activeTracker:
            self.activeTracker.dt = dt
            self.activeTracker.update()

    def reset(self):
        self.tLast = time.time()
        if self.activeTracker:
            self.activeTracker.reset()

    def getModeActions(self):
        if self.activeTracker:
            return self.activeTracker.actions
        return []

    def onModeAction(self, actionName):
        if self.activeTracker:
            self.activeTracker.onAction(actionName)

    def getModeProperties(self):
        if self.activeTracker:
            return self.activeTracker.properties
        return None

    def onTargetFrameModified(self, frame):
        self.update()

    def initTracker(self):

        self.timer.stop()
        self.activeTracker = (
            self.trackerClass(self.view, self.targetFrame) if (self.trackerClass and self.targetFrame) else None
        )
        self.reset()
        self.update()

        if self.activeTracker:
            minimumUpdateRate = self.activeTracker.getMinimumUpdateRate()
            if minimumUpdateRate > 0:
                self.timer.targetFps = minimumUpdateRate
                self.timer.start()
#.........这里部分代码省略.........