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 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()
from ddapp.consoleapp import ConsoleApp from ddapp import lcmspy from ddapp import lcmUtils from ddapp import simpletimer as st app = ConsoleApp() app.setupGlobals(globals()) if app.getTestingInteractiveEnabled(): app.showPythonConsole() lcmspy.findLCMModulesInSysPath() timer = st.SimpleTimer() stats = {} channelToMsg = {} items = {} def item(r, c): rowDict = items.setdefault(r, {}) try: return rowDict[c] except KeyError: i = QtGui.QTableWidgetItem('') table.setItem(r, c, i) rowDict[c] = i return i