Python TimerCallback.stop方法代码示例

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

#.........这里部分代码省略.........
    def getControllerTypeFromCounter(self, counter):
        name = self.controllerTypeOrder[0]

        for controllerType in self.controllerTypeOrder[1:]:
            if counter >= self.idxDict[controllerType]:
                name = controllerType

        return name

    def setRobotFrameState(self, x, y, theta):
        t = vtk.vtkTransform()
        t.Translate(x, y, 0.0)
        t.RotateZ(np.degrees(theta))
        self.robot.getChildFrame().copyFrame(t)

    # returns true if we are in collision
    def checkInCollision(self, raycastDistance=None):
        if raycastDistance is None:
            self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2])
            raycastDistance = self.Sensor.raycastAll(self.frame)

        if np.min(raycastDistance) < self.collisionThreshold:
            return True
        else:
            return False

    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 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)
        x, y, theta = self.stateOverTime[idx]
        self.setRobotFrameState(x, y, theta)
        self.sliderMovedByPlayTimer = False

    def onPlayButton(self):

        if self.playTimer.isActive():
            self.onPauseButton()

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