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()
class RosSubscriberManager(object): """A helper class for managing rospy subscribers and dispatching callbacks.""" def __init__(self): self.msgs = {} self.pending_msgs = {} self.counters = {} self.subs = OrderedDict() self.callbacks = OrderedDict() def init_node(self): rospy.init_node('director_rospy_node', anonymous=True) def on_idle(): time.sleep(0.0001) self.idle_timer = TimerCallback(callback=on_idle, targetFps=100) self.idle_timer.start() self.dispatch_timer = TimerCallback(targetFps=30) self.dispatch_timer.callback = self.on_dispatch_timer self.dispatch_timer.start() def get_estimated_topic_hz(self, topic_name): assert topic_name in self.counters return self.counters[topic_name].getAverageFPS() def get_latest_msg(self, topic_name): return self.msgs.get(topic_name) def subscribe(self, topic_name, message_type, message_function=None, call_on_thread=False): def on_message(msg): self.msgs[topic_name] = msg self.counters[topic_name].tick() if call_on_thread and message_function: message_function(topic_name, msg) else: self.pending_msgs[topic_name] = msg self.counters[topic_name] = FPSCounter() self.callbacks[topic_name] = message_function self.subs[topic_name] = rospy.Subscriber(topic_name, message_type, on_message) def handle_pending_message(self, topic_name): msg = self.pending_msgs.pop(topic_name, None) if msg is not None: callback = self.callbacks.get(topic_name) if callback: callback(topic_name, msg) def handle_pending_messages(self): for topic_name in list(self.pending_msgs.keys()): self.handle_pending_message(topic_name) def on_dispatch_timer(self): self.handle_pending_messages()
class LCMForceDisplay(object): ''' Displays foot force sensor signals in a status bar widget or label widget ''' def onRobotState(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, lcmbotcore.robot_state_t, self.onRobotState) 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)
class LCMForceDisplay(object): ''' Displays foot force sensor signals in a status bar widget or label widget ''' def onRobotState(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, lcmbotcore.robot_state_t, self.onRobotState) 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 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 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 start(self): if self.reader is None: self.reader = drc.vtkLidarSource() self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 80.0) self.reader.SetHeightRange(-80.0, 80.0) self.reader.Start() TimerCallback.start(self)
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
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 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.SetHeightRange(-80.0, 80.0) self.reader.Start() self.setIntensityRange(400, 4000) TimerCallback.start(self)
def start(self): if self.reader is None: self.reader = drc.vtkLidarSource() self.reader.subscribe(self.channelName) self.reader.setCoordinateFrame(self.coordinateFrame) self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 80.0) self.reader.SetHeightRange(-80.0, 80.0) self.reader.Start() TimerCallback.start(self)
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 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()
class TaskRunner(object): def __init__(self): self.interval = 1 / 60.0 sys.setcheckinterval(1000) #sys.setswitchinterval(self.interval) # sys.setswitchinterval is only Python 3 self.taskQueue = asynctaskqueue.AsyncTaskQueue() self.pendingTasks = [] self.threads = [] self.timer = TimerCallback(callback=self._onTimer, targetFps=1 / self.interval) def _onTimer(self): # Give up control to another python thread in self.threads # that might be running time.sleep(self.interval) # add all tasks in self.pendingTasks to the AsyncTaskQueue if self.pendingTasks: while True: try: self.taskQueue.addTask(self.pendingTasks.pop(0)) except IndexError: break # start the AsyncTaskQueue if it's not already running if self.taskQueue.tasks and not self.taskQueue.isRunning: self.taskQueue.start() # check which threads are live liveThreads = [] for t in self.threads: if t.is_alive(): liveThreads.append(t) # only retain the live threads self.threads = liveThreads # if no liveThreads then stop the timer if len(self.threads) == 0: self.timer.stop() def callOnMain(self, func, *args, **kwargs): self.pendingTasks.append(lambda: func(*args, **kwargs)) self.timer.start() def callOnThread(self, func, *args, **kwargs): t = Thread(target=lambda: func(*args, **kwargs)) self.threads.append(t) t.start() self.timer.start()
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 TaskRunner(object): def __init__(self): self.interval = 1/60.0 sys.setcheckinterval(1000) try: sys.setswitchinterval(self.interval) except AttributeError: # sys.setswitchinterval is only python3 pass self.taskQueue = asynctaskqueue.AsyncTaskQueue() self.pendingTasks = [] self.threads = [] self.timer = TimerCallback(callback=self._onTimer, targetFps=1/self.interval) # call timer.start here to initialize the QTimer now on the main thread self.timer.start() def _onTimer(self): # add all tasks in self.pendingTasks to the AsyncTaskQueue if self.pendingTasks: while True: try: self.taskQueue.addTask(self.pendingTasks.pop(0)) except IndexError: break # start the AsyncTaskQueue if it's not already running if self.taskQueue.tasks and not self.taskQueue.isRunning: self.taskQueue.start() # only retain the live threads self.threads = [t for t in self.threads if t.is_alive()] if self.threads: # Give up control to other python threads that are running time.sleep(self.interval) def callOnMain(self, func, *args, **kwargs): self.pendingTasks.append(lambda: func(*args, **kwargs)) def callOnThread(self, func, *args, **kwargs): t = Thread(target=lambda: func(*args, **kwargs)) self.threads.append(t) t.start() return t
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 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 EstRobotStatePublisher(object): def __init__(self, robotSystem): self.robotSystem = robotSystem self.timer = TimerCallback(targetFps=25) self.timer.callback = self.publishEstRobotState def publishEstRobotState(self): q = self.robotSystem.robotStateJointController.q stateMsg = robotstate.drakePoseToRobotState(q) stateMsg.utime = utimeUtil.getUtime() lcmUtils.publish("EST_ROBOT_STATE", stateMsg) def start(self): self.timer.start() def stop(self): self.timer.stop()
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 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)
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()
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()
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("MULTISENSE_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())
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
class AnimatePropertyValue(object): ''' This class is used to ramp a scalar or vector property from its current value to a target value using linear inteprolation. For example: obj = getSomeObject() # fade the Alpha property to 0.0 AnimatePropertyValue(obj, 'Alpha', 0.0).start() ''' def __init__(self, obj, propertyName, targetValue, animationTime=1.0): self.obj = obj self.propertyName = propertyName self.animationTime = animationTime self.targetValue = targetValue self.timer = TimerCallback() def start(self): self.startTime = time.time() self.startValue = np.array(self.obj.getProperty(self.propertyName)) self.targetValue = np.array(self.targetValue) self.timer.callback = self.tick self.timer.start() def tick(self): elapsed = time.time() - self.startTime p = elapsed/self.animationTime if p > 1.0: p = 1.0 newValue = self.startValue + (self.targetValue - self.startValue)*p self.obj.setProperty(self.propertyName, newValue) if p == 1.0: self.timer.callback = None return False
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
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())
ql.setOrientation(QtCore.Qt.Vertical) return ql statusLabel = spawnBasicLabel() l.addWidget(wrapInVTitledItem("Behavior", [statusLabel])) def statusUpdate(): behavior = atlasDriver.getCurrentBehaviorName() statusLabel.setText(behavior) if (behavior != "user"): statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white") else: statusLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:white; color:black") statusTimer = TimerCallback(targetFps=FPS) statusTimer.callback = statusUpdate statusTimer.start() controllerLabel = spawnBasicLabel() l.addWidget(wrapInVTitledItem("Controller", [controllerLabel])) def controllerUpdate(): status = atlasDriver.getControllerStatus() if not status: status = "Unknown" if len(status) > 6: status = status[0:6] rate = atlasDriver.getControllerRate() if not rate: rate = 0.0 controllerLabel.setText("%s\n%06.1fhz" % (status, rate)) if (rate < 500): controllerLabel.setStyleSheet(LABEL_DEFAULT_STYLE_SHEET + "background-color:red; color:white")
class Simulator(object): def __init__( self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True, ): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 0.4 self.randomSeed = 5 self.Sensor_rayLength = 8 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() self.XVelocity_drawing = 0.0 self.YVelocity_drawing = 0.0 def initializeOptions(self): self.options = dict() self.options["World"] = dict() self.options["World"]["obstaclesInnerFraction"] = 0.98 self.options["World"]["randomSeed"] = 40 self.options["World"]["percentObsDensity"] = 0.0 self.options["World"]["nonRandomWorld"] = True self.options["World"]["circleRadius"] = 1.0 self.options["World"]["scale"] = 1 self.options["Sensor"] = dict() self.options["Sensor"]["rayLength"] = 10 self.options["Sensor"]["numRays"] = 50 self.options["Car"] = dict() self.options["Car"]["velocity"] = 4.0 self.options["dt"] = 0.05 self.options["runTime"] = dict() self.options["runTime"]["defaultControllerTime"] = 100 def setDefaultOptions(self): defaultOptions = dict() defaultOptions["World"] = dict() defaultOptions["World"]["obstaclesInnerFraction"] = 0.98 defaultOptions["World"]["randomSeed"] = 40 defaultOptions["World"]["percentObsDensity"] = 30 defaultOptions["World"]["nonRandomWorld"] = True defaultOptions["World"]["circleRadius"] = 1.75 defaultOptions["World"]["scale"] = 2.5 defaultOptions["Sensor"] = dict() defaultOptions["Sensor"]["rayLength"] = 10 defaultOptions["Sensor"]["numRays"] = 51 defaultOptions["Car"] = dict() defaultOptions["Car"]["velocity"] = 20 defaultOptions["dt"] = 0.05 defaultOptions["runTime"] = dict() defaultOptions["runTime"]["defaultControllerTime"] = 100 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap["default"] = [0, 1, 0] def initialize(self): self.dt = self.options["dt"] self.controllerTypeOrder = ["default"] self.setDefaultOptions() self.Sensor = SensorObj( rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"] ) self.SensorApproximator = SensorApproximatorObj( numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"] ) self.Controller = ControllerObj(self.Sensor, self.SensorApproximator) self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"]) self.Controller.initializeVelocity(self.Car.v) self.ActionSet = ActionSetObj() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName("world")) self.world = World.buildLineSegmentTestWorld( percentObsDensity=self.options["World"]["percentObsDensity"], circleRadius=self.options["World"]["circleRadius"], nonRandom=self.options["World"]["nonRandomWorld"], scale=self.options["World"]["scale"], randomSeed=self.options["World"]["randomSeed"], obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"], ) om.removeFromObjectModel(om.findObjectByName("robot")) self.robot, self.frame = World.buildRobot() self.frame = self.robot.getChildFrame() self.frame.setProperty("Scale", 3) # self.frame.setProperty('Visible', False) # self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"] self.Car.setFrame(self.frame) print "Finished initialization" def runSingleSimulation(self, controllerType="default", simulationCutoff=None): # self.setRandomCollisionFreeInitialState() self.setInitialStateAtZero() currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) firstRaycast = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame) # self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) # self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) # self.Sensor.setLocator(self.LineSegmentLocator) nextRaycast = np.zeros(self.Sensor.numRays) # record the reward data runData = dict() startIdx = self.counter thisRunIndex = 0 NMaxSteps = 100 while self.counter < self.numTimesteps - 1: thisRunIndex += 1 idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx, :] = currentCarState x = self.stateOverTime[idx, 0] y = self.stateOverTime[idx, 1] self.setRobotFrameState(x, y, 0.0) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycast = self.Sensor.raycastAll(self.frame) self.raycastData[idx, :] = currentRaycast S_current = (currentCarState, currentRaycast) if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") if controllerType in ["default", "defaultRandom"]: controlInput, controlInputIdx = self.Controller.computeControlInput( currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False ) self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) print "NEXTCARSTATE is ", nextCarState x = nextCarState[0] y = nextCarState[1] self.setRobotFrameState(x, y, 0.0) nextRaycast = self.Sensor.raycastAll(self.frame) # Compute the next control input S_next = (nextCarState, nextRaycast) if controllerType in ["default", "defaultRandom"]: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput( nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False ) # bookkeeping currentCarState = nextCarState currentRaycast = nextRaycast self.counter += 1 # break if we are in collision if self.checkInCollision(nextRaycast): if self.verbose: print "Had a collision, terminating simulation" break if thisRunIndex > NMaxSteps: print "was safe during N steps" break if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter, :] = currentCarState self.raycastData[self.counter, :] = currentRaycast # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): self.controllerTypeOrder = ["default"] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict["default"] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps) while (self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1: self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType="default", simulationCutoff=simCutoff) runData["startIdx"] = startIdx runData["controllerType"] = "default" runData["duration"] = self.counter - runData["startIdx"] runData["endIdx"] = self.counter runData["runNumber"] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0 : self.counter + 1, :] self.raycastData = self.raycastData[0 : self.counter + 1, :] self.controlInputData = self.controlInputData[0 : self.counter + 1] self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * ( self.numTicks - self.nextTickIdx ), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 # + np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x, y, 0.0, 0.0) self.setRobotFrameState(x, y, theta) print "In loop" if not self.checkInCollision(): break return x, y, theta def setInitialStateAtZero(self): x = 0.0 y = 0.0 theta = 0.0 self.Car.setCarState(x, y, 0.0, 0.0) self.setRobotFrameState(x, y, theta) return x, y, theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0] y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0] theta = np.random.uniform(0, 2 * np.pi, 1)[0] self.Car.setCarState(x, y, theta) self.setRobotFrameState(x, y, theta) if not self.checkInCollision(): break return x, y, theta 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) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0 self.drawFirstIntersections(self.frame, firstRaycast) randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal") randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles") randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World") buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles) l.addWidget(buildWorldFromRandomObstaclesButton) findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)") findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton) l.addWidget(findLocalGoalButton) drawActionSetButton = QtGui.QPushButton("Draw Action Set") drawActionSetButton.connect("clicked()", self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton("Simulate") runSimButton.connect("clicked()", self.onRunSimButton) l.addWidget(runSimButton) 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 # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() 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() cameracontrolpanel.CameraControlPanel(self.view).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.onRandomObstaclesButton() self.app.start() def drawFirstIntersections(self, frame, firstRaycast): origin = np.array(frame.transform.GetPosition()) d = DebugData() firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast) for i in xrange(self.Sensor.numRays): endpoint = firstRaycastLocations[i, :] if firstRaycast[i] >= self.Sensor.rayLength - 0.1: d.addLine(origin, endpoint, color=[0, 1, 0]) else: d.addLine(origin, endpoint, color=[1, 0, 0]) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255") self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.locator = self.LineSegmentLocator self.Sensor.setLocator(self.LineSegmentLocator) def updateDrawIntersection(self, frame, locator="None"): if locator == "None": locator = self.locator origin = np.array(frame.transform.GetPosition()) # print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) # print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255") # camera = self.view.camera() # camera.SetFocalPoint(frame.transform.GetPosition()) # camera.SetPosition(frame.transform.TransformPoint((-30,0,10))) 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 # if raycastDistance[(len(raycastDistance)+1)/2] < 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, xdot, ydot = self.stateOverTime[idx] self.setRobotFrameState(x, y, 0) self.sliderMovedByPlayTimer = False def onXVelocityChanged(self, value): self.XVelocity_drawing = value self.onDrawActionSetButton() print "x velocity changed to ", value def onYVelocityChanged(self, value): print value self.YVelocity_drawing = -value self.onDrawActionSetButton() print "y velocity changed to ", -value def onShowSensorsButton(self): print "I pressed the show sensors button" self.setInitialStateAtZero() firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0 self.drawFirstIntersections(self.frame, firstRaycast) def onRandomObstaclesButton(self): print "random obstacles button pressed" self.setInitialStateAtZero() self.world = World.buildLineSegmentTestWorld( percentObsDensity=8.0, circleRadius=self.options["World"]["circleRadius"], nonRandom=False, scale=self.options["World"]["scale"], randomSeed=self.options["World"]["randomSeed"], obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"], ) self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.updateDrawIntersection(self.frame, locator=self.locator) def onRandomGlobalGoalButton(self): print "random global goal button pressed" self.globalGoal = World.buildGlobalGoal() def onBuildWorldFromRandomObstacles(self): distances = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances) self.polygon_initial_distances = distances self.polygon_initial_raycastLocations = firstRaycastLocations self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.locator = self.LineSegmentLocator self.Sensor.setLocator(self.LineSegmentLocator) self.updateDrawIntersection(self.frame) def onFindLocalGoalButton(self): print "find local goal button pressed" local_goal = self.findLocalGoal() print local_goal, "is my local goal" self.localGoal = World.placeLocalGoal(local_goal) def findLocalGoal(self): biggest_gap_width = 0 biggest_gap_start_index = 0 current_gap_width = 0 current_gap_start_index = 0 for index, value in enumerate(self.polygon_initial_distances): # if still in a gap if value == self.Sensor.rayLength: current_gap_width += 1 # else terminate counting for this gap else: if current_gap_width > biggest_gap_width: biggest_gap_width = current_gap_width biggest_gap_start_index = current_gap_start_index current_gap_width = 0 current_gap_start_index = index + 1 if current_gap_width > biggest_gap_width: biggest_gap_width = current_gap_width biggest_gap_start_index = current_gap_start_index middle_index_of_gap = biggest_gap_start_index + biggest_gap_width / 2 return self.polygon_initial_raycastLocations[middle_index_of_gap, :] def onDrawActionSetButton(self): print "drawing action set" # self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing) self.ActionSet.computeAllPositions(self.XVelocity_drawing, self.YVelocity_drawing) # self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def onRunSimButton(self): self.runBatchSimulation() self.saveToFile("latest") def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print "play" self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print "pause" self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = "data/" + filename + ".out" my_shelf = shelve.open(filename, "n") my_shelf["options"] = self.options my_shelf["simulationData"] = self.simulationData my_shelf["stateOverTime"] = self.stateOverTime my_shelf["raycastData"] = self.raycastData my_shelf["controlInputData"] = self.controlInputData my_shelf["numTimesteps"] = self.numTimesteps my_shelf["idxDict"] = self.idxDict my_shelf["counter"] = self.counter my_shelf.close() def run(self, launchApp=True): self.counter = 1 # for use in playback self.dt = self.options["dt"] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, self.dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps + 1, 4)) self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays)) self.controlInputData = np.zeros((maxNumTimesteps + 1, 2)) self.numTimesteps = maxNumTimesteps # self.runBatchSimulation() if launchApp: self.setupPlayback() @staticmethod def loadFromFile(filename): filename = "data/" + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf["options"] sim.initialize() sim.simulationData = my_shelf["simulationData"] sim.stateOverTime = np.array(my_shelf["stateOverTime"]) sim.raycastData = np.array(my_shelf["raycastData"]) sim.controlInputData = np.array(my_shelf["controlInputData"]) sim.numTimesteps = my_shelf["numTimesteps"] sim.idxDict = my_shelf["idxDict"] sim.counter = my_shelf["counter"] my_shelf.close() return sim
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.useDevelopmentColors() 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 = False 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): ''' Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden' e.g. can hide all plan playback with 'hidden' ''' self.ui.viewModeCombo.setCurrentIndex( self.ui.viewModeCombo.findText(mode)) def getPlaybackSpeed(self): s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '') if '/' in s: n, d = s.split('/') return float(n) / float(d) return float(s) def getInterpolationMethod(self): return str(self.ui.interpolationCombo.currentText) def getNumberOfSamples(self): return self.ui.samplesSpinBox.value def viewModeChanged(self): viewMode = self.getViewMode() if viewMode == 'continuous': playbackVisible = True samplesVisible = False interpolationVisible = True elif viewMode == 'frames': playbackVisible = False samplesVisible = True interpolationVisible = True elif viewMode == 'hidden': playbackVisible = False samplesVisible = False interpolationVisible = False else: raise Exception('Unexpected view mode') self.ui.samplesLabel.setVisible(samplesVisible) self.ui.samplesSpinBox.setVisible(samplesVisible) self.ui.interpolationLabel.setVisible(interpolationVisible) self.ui.interpolationCombo.setVisible(interpolationVisible) self.ui.playbackSpeedLabel.setVisible(playbackVisible) self.ui.playbackSpeedCombo.setVisible(playbackVisible) self.ui.playbackSlider.setEnabled(playbackVisible) self.ui.animateButton.setVisible(playbackVisible) self.ui.timeLabel.setVisible(playbackVisible) self.hidePlan() if self.plan: if viewMode == 'continuous' and self.autoPlay: self.startAnimation() elif viewMode == 'frames': self.updatePlanFrames() def playbackSpeedChanged(self): self.planPlayback.playbackSpeed = self.getPlaybackSpeed() def getPlaybackTime(self): sliderValue = self.ui.playbackSlider.value return (sliderValue / 1000.0) * self.endTime def updateTimeLabel(self): playbackTime = self.getPlaybackTime() self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime def playbackSliderValueChanged(self): self.updateTimeLabel() self.showPoseAtTime(self.getPlaybackTime()) def interpolationChanged(self): methods = { 'linear': 'slinear', 'cubic spline': 'cubic', 'pchip': 'pchip' } self.planPlayback.interpolationMethod = methods[ self.getInterpolationMethod()] self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan( self.plan) self.updatePlanFrames() def numberOfSamplesChanged(self): self.updatePlanFrames() def animateClicked(self): self.startAnimation() def hideClicked(self): if self.ui.hideButton.text == 'hide': self.ui.playbackFrame.setEnabled(False) self.hidePlan() self.ui.hideButton.text = 'show' self.ui.executeButton.setEnabled(False) if not self.plan: self.ui.hideButton.setEnabled(False) else: self.ui.playbackFrame.setEnabled(True) self.ui.hideButton.text = 'hide' self.ui.hideButton.setEnabled(True) self.ui.executeButton.setEnabled(True) self.viewModeChanged() self.updateButtonColor() def executeClicked(self): self.executePlan() def executePlan(self, visOnly=False, overrideInfeasibleCheck=False, overrideSupportsCheck=False): if visOnly: _, poses = self.planPlayback.getPlanPoses(self.plan) self.onPlanCommitted(self.plan) self.robotStateJointController.setPose('EST_ROBOT_STATE', poses[-1]) else: if (self.isPlanFeasible() or overrideInfeasibleCheck) and ( not self.isPlanAPlanWithSupports() or overrideSupportsCheck): self.manipPlanner.commitManipPlan(self.plan) def onPlanCommitted(self, plan): self.setPlan(None) self.hideClicked() def stopClicked(self): self.stopAnimation() self.manipPlanner.sendPlanPause() def isPlanFeasible(self): plan = robotstate.asRobotPlan(self.plan) return plan is not None and (max(plan.plan_info) < 10 and min(plan.plan_info) >= 0) def getPlanInfo(self, plan): plan = robotstate.asRobotPlan(self.plan) return max(plan.plan_info) def isPlanAPlanWithSupports(self): return hasattr( self.plan, 'support_sequence') or self.manipPlanner.publishPlansWithSupports def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames() def showPlanFrames(self): self.planFramesObj.setProperty('Visible', True) self.robotStateModel.setProperty('Visible', False) self.playbackRobotModel.setProperty('Visible', False) def startAnimation(self): self.showPlaybackModel() self.stopAnimation() self.ui.playbackSlider.value = 0 self.animationClock.reset() self.animationTimer.start() self.updateAnimation() def stopAnimation(self): self.animationTimer.stop() def showPlaybackModel(self): self.robotStateModel.setProperty('Visible', True) self.playbackRobotModel.setProperty('Visible', True) self.playbackRobotModel.setProperty( 'Color Mode', 1 if self.playbackRobotModelUseTextures else 0) self.robotStateModel.setProperty('Alpha', self.robotStateModelDisplayAlpha) self.playbackRobotModel.setProperty( 'Alpha', self.playbackRobotModelDisplayAlpha) if self.planFramesObj: self.planFramesObj.setProperty('Visible', False) def hidePlan(self): self.stopAnimation() self.ui.playbackSlider.value = 0 if self.planFramesObj: self.planFramesObj.setProperty('Visible', False) if self.playbackRobotModel: self.playbackRobotModel.setProperty('Visible', False) self.robotStateModel.setProperty('Visible', True) self.robotStateModel.setProperty('Alpha', 1.0) def showPoseAtTime(self, time): pose = self.poseInterpolator(time) self.playbackJointController.setPose('plan_playback', pose) def updateAnimation(self): tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed if tNow > self.endTime: tNow = self.endTime sliderValue = int(1000.0 * tNow / self.endTime) self.ui.playbackSlider.blockSignals(True) self.ui.playbackSlider.value = sliderValue self.ui.playbackSlider.blockSignals(False) self.updateTimeLabel() self.showPoseAtTime(tNow) return tNow < self.endTime def updateButtonColor(self): if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible( ): styleSheet = 'background-color:red' elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports( ): styleSheet = 'background-color:orange' else: styleSheet = '' self.ui.executeButton.setStyleSheet(styleSheet) def setPlan(self, plan): self.ui.playbackSlider.value = 0 self.ui.timeLabel.text = 'Time: 0.00 s' self.ui.planNameLabel.text = '' self.plan = plan self.endTime = 1.0 self.updateButtonColor() if not self.plan: return planText = 'Plan: %d. %.2f seconds' % ( plan.utime, self.planPlayback.getPlanElapsedTime(plan)) self.ui.planNameLabel.text = planText self.startTime = 0.0 self.endTime = self.planPlayback.getPlanElapsedTime(plan) self.interpolationChanged() info = self.getPlanInfo(plan) app.displaySnoptInfo(info) if self.ui.hideButton.text == 'show': self.hideClicked() else: self.viewModeChanged() self.updateButtonColor() if self.autoPlay and self.widget.parent() is not None: self.widget.parent().show()
class ImageWidget(object): def __init__(self, imageManager, imageName, view, visible=True): self.view = view self.imageManager = imageManager self.imageName = imageName self.visible = visible self.updateUtime = 0 self.initialized = False self.imageWidget = vtk.vtkLogoWidget() imageRep = self.imageWidget.GetRepresentation() self.imageWidget.ResizableOff() self.imageWidget.SelectableOn() imageRep.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)) imageRep.SetImage(self.flip.GetOutput()) self.eventFilter = PythonQt.dd.ddPythonEventFilter() self.view.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize) self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.onResizeEvent) self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def setWidgetSize(self, desiredWidth=400): image = self.imageManager.getImage(self.imageName) dims = image.GetDimensions() if 0.0 in dims: return aspectRatio = float(dims[0]) / dims[1] imageWidth, imageHeight = desiredWidth, desiredWidth / aspectRatio viewWidth, viewHeight = self.view.width, self.view.height rep = self.imageWidget.GetBorderRepresentation() rep.SetShowBorderToOff() coord = rep.GetPositionCoordinate() coord2 = rep.GetPosition2Coordinate() coord.SetCoordinateSystemToDisplay() coord2.SetCoordinateSystemToDisplay() coord.SetValue(0, viewHeight - imageHeight) coord2.SetValue(imageWidth, imageHeight) self.view.render() def onResizeEvent(self): self.setWidgetSize(400) def setImageName(self, imageName): self.imageName = imageName self.flip.SetInput(imageManager.getImage(imageName)) def setOpacity(self, opacity=1.0): self.imageWidget.GetRepresentation().GetImageProperty().SetOpacity(opacity) def hide(self): self.visible = False self.imageWidget.Off() self.view.render() def show(self): self.visible = True if self.haveImage(): self.imageWidget.On() self.view.render() def haveImage(self): image = self.imageManager.getImage(self.imageName) dims = image.GetDimensions() return 0.0 not in dims def updateView(self): if not self.visible or not self.view.isVisible(): return currentUtime = self.imageManager.updateImage(self.imageName) if currentUtime != self.updateUtime: self.updateUtime = currentUtime self.flip.Update() self.view.render() if not self.initialized and self.visible and self.haveImage(): self.show() self.setWidgetSize(400) self.initialized = True
class CameraImageView(object): def __init__(self, imageManager, imageName, viewName=None, view=None): imageManager.addImage(imageName) self.cameraRoll = None self.imageManager = imageManager self.viewName = viewName or imageName self.imageName = imageName self.imageInitialized = False self.updateUtime = 0 self.initView(view) self.initEventFilter() def getImagePixel(self, displayPoint, restrictToImageDimensions=True): worldPoint = [0.0, 0.0, 0.0, 0.0] vtk.vtkInteractorObserver.ComputeDisplayToWorld( self.view.renderer(), displayPoint[0], displayPoint[1], 0, worldPoint ) imageDimensions = self.getImage().GetDimensions() if ( 0.0 <= worldPoint[0] <= imageDimensions[0] and 0.0 <= worldPoint[1] <= imageDimensions[1] or not restrictToImageDimensions ): return [worldPoint[0], worldPoint[1], 0.0] else: return None def getWorldPositionAndRay(self, imagePixel, imageUtime=None): """ Given an XY image pixel, computes an equivalent ray in the world coordinate system using the camera to local transform at the given imageUtime. If imageUtime is None, then the utime of the most recent image is used. Returns the camera xyz position in world, and a ray unit vector. """ if imageUtime is None: imageUtime = self.imageManager.getUtime(self.imageName) # input is pixel u,v, output is unit x,y,z in camera coordinates cameraPoint = self.imageManager.queue.unprojectPixel(self.imageName, imagePixel[0], imagePixel[1]) cameraToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform(self.imageName, "local", imageUtime, cameraToLocal) p = np.array(cameraToLocal.TransformPoint(cameraPoint)) cameraPosition = np.array(cameraToLocal.GetPosition()) ray = p - cameraPosition ray /= np.linalg.norm(ray) return cameraPosition, ray def filterEvent(self, obj, event): if self.eventFilterEnabled and event.type() == QtCore.QEvent.MouseButtonDblClick: self.eventFilter.setEventHandlerResult(True) elif event.type() == QtCore.QEvent.KeyPress: if str(event.text()).lower() == "p": self.eventFilter.setEventHandlerResult(True) elif str(event.text()).lower() == "r": self.eventFilter.setEventHandlerResult(True) self.resetCamera() def onRubberBandPick(self, obj, event): displayPoints = self.interactorStyle.GetStartPosition(), self.interactorStyle.GetEndPosition() imagePoints = [vis.pickImage(point, self.view)[1] for point in displayPoints] sendFOVRequest(self.imageName, imagePoints) def getImage(self): return self.imageManager.getImage(self.imageName) def initView(self, view): self.view = view or app.getViewManager().createView(self.viewName, "VTK View") self.view.installImageInteractor() self.interactorStyle = self.view.renderWindow().GetInteractor().GetInteractorStyle() self.interactorStyle.AddObserver("SelectionChangedEvent", self.onRubberBandPick) self.imageActor = vtk.vtkImageActor() self.imageActor.SetInput(self.getImage()) self.imageActor.SetVisibility(False) self.view.renderer().AddActor(self.imageActor) self.view.orientationMarkerWidget().Off() self.view.backgroundRenderer().SetBackground(0, 0, 0) self.view.backgroundRenderer().SetBackground2(0, 0, 0) self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def initEventFilter(self): self.eventFilter = PythonQt.dd.ddPythonEventFilter() qvtkwidget = self.view.vtkWidget() qvtkwidget.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick) self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress) self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.filterEvent) self.eventFilterEnabled = True def setCameraRoll(self, roll): self.cameraRoll = roll self.resetCamera() def resetCamera(self): camera = self.view.camera() camera.ParallelProjectionOn() camera.SetFocalPoint(0, 0, 0) camera.SetPosition(0, 0, -1) camera.SetViewUp(0, -1, 0) if self.cameraRoll is not None: camera.SetRoll(self.cameraRoll) self.view.resetCamera() self.fitImageToView() self.view.render() def fitImageToView(self): camera = self.view.camera() image = self.getImage() imageWidth, imageHeight, _ = image.GetDimensions() viewWidth, viewHeight = self.view.renderWindow().GetSize() aspectRatio = float(viewWidth) / viewHeight parallelScale = max(imageWidth / aspectRatio, imageHeight) / 2.0 camera.SetParallelScale(parallelScale) def setImageName(self, imageName): if imageName == self.imageName: return assert self.imageManager.hasImage(imageName) self.imageName = imageName self.imageInitialized = False self.updateUtime = 0 self.imageActor.SetInput(self.imageManager.getImage(self.imageName)) self.imageActor.SetVisibility(False) self.view.render() def updateView(self): if not self.view.isVisible(): return currentUtime = self.imageManager.updateImage(self.imageName) if currentUtime != self.updateUtime: self.updateUtime = currentUtime self.view.render() if not self.imageInitialized and self.imageActor.GetInput().GetDimensions()[0]: self.imageActor.SetVisibility(True) self.resetCamera() self.imageInitialized = True
class PlanPlayback(object): def __init__(self): self.animationCallback = None self.animationTimer = None self.interpolationMethod = "slinear" self.playbackSpeed = 1.0 self.jointNameRegex = "" @staticmethod def getPlanPoses(msgOrList): if isinstance(msgOrList, list): messages = msgOrList allPoseTimes, allPoses = PlanPlayback.getPlanPoses(messages[0]) for msg in messages[1:]: poseTimes, poses = PlanPlayback.getPlanPoses(msg) poseTimes += allPoseTimes[-1] allPoseTimes = np.hstack((allPoseTimes, poseTimes[1:])) allPoses += poses[1:] return allPoseTimes, allPoses else: msg = asRobotPlan(msgOrList) poses = [] poseTimes = [] for plan in msg.plan: pose = robotstate.convertStateMessageToDrakePose(plan) poseTimes.append(plan.utime / 1e6) poses.append(pose) return np.array(poseTimes), poses @staticmethod def getPlanElapsedTime(msg): msg = asRobotPlan(msg) startTime = msg.plan[0].utime endTime = msg.plan[-1].utime return (endTime - startTime) / 1e6 def stopAnimation(self): if self.animationTimer: self.animationTimer.stop() def setInterpolationMethod(method): self.interpolationMethod = method def playPlan(self, msg, jointController): self.playPlans([msg], jointController) def playPlans(self, messages, jointController): assert len(messages) poseTimes, poses = self.getPlanPoses(messages) self.playPoses(poseTimes, poses, jointController) def getPoseInterpolatorFromPlan(self, message): poseTimes, poses = self.getPlanPoses(message) return self.getPoseInterpolator(poseTimes, poses) def getPoseInterpolator(self, poseTimes, poses, unwrap_rpy=True): if unwrap_rpy: poses = np.array(poses, copy=True) poses[:, 3:6] = np.unwrap(poses[:, 3:6], axis=0) if self.interpolationMethod in ["slinear", "quadratic", "cubic"]: f = scipy.interpolate.interp1d(poseTimes, poses, axis=0, kind=self.interpolationMethod) elif self.interpolationMethod == "pchip": f = scipy.interpolate.PchipInterpolator(poseTimes, poses, axis=0) return f def getPlanPoseMeshes(self, messages, jointController, robotModel, numberOfSamples): poseTimes, poses = self.getPlanPoses(messages) f = self.getPoseInterpolator(poseTimes, poses) sampleTimes = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples) meshes = [] for sampleTime in sampleTimes: pose = f(sampleTime) jointController.setPose("plan_playback", pose) polyData = vtk.vtkPolyData() robotModel.model.getModelMesh(polyData) meshes.append(polyData) return meshes def showPoseAtTime(self, time, jointController, poseInterpolator): pose = poseInterpolator(time) jointController.setPose("plan_playback", pose) 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 picklePlan(self, filename, msg): poseTimes, poses = self.getPlanPoses(msg) pickle.dump((poseTimes, poses), open(filename, "w")) def getMovingJointNames(self, msg): poseTimes, poses = self.getPlanPoses(msg) diffs = np.diff(poses, axis=0) jointIds = np.unique(np.where(diffs != 0.0)[1]) jointNames = [ robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds ] return jointNames def plotPlan(self, msg): poseTimes, poses = self.getPlanPoses(msg) self.plotPoses(poseTimes, poses) def plotPoses(self, poseTimes, poses): import matplotlib.pyplot as plt poses = np.array(poses) if self.jointNameRegex: jointIds = range(poses.shape[1]) else: diffs = np.diff(poses, axis=0) jointIds = np.unique(np.where(diffs != 0.0)[1]) jointNames = [ robotstate.getDrakePoseJointNames()[jointId] for jointId in jointIds ] jointTrajectories = [poses[:, jointId] for jointId in jointIds] seriesNames = [] sampleResolutionInSeconds = 0.01 numberOfSamples = (poseTimes[-1] - poseTimes[0]) / sampleResolutionInSeconds xnew = np.linspace(poseTimes[0], poseTimes[-1], numberOfSamples) fig = plt.figure() ax = fig.add_subplot(111) for jointId, jointName, jointTrajectory in zip(jointIds, jointNames, jointTrajectories): if self.jointNameRegex and not re.match(self.jointNameRegex, jointName): continue x = poseTimes y = jointTrajectory y = np.rad2deg(y) if self.interpolationMethod in ["slinear", "quadratic", "cubic"]: f = scipy.interpolate.interp1d(x, y, kind=self.interpolationMethod) elif self.interpolationMethod == "pchip": f = scipy.interpolate.PchipInterpolator(x, y) ax.plot(x, y, "ko") seriesNames.append(jointName + " points") ax.plot(xnew, f(xnew), "-") seriesNames.append(jointName + " " + self.interpolationMethod) ax.legend(seriesNames, loc="upper right").draggable() ax.set_xlabel("time (s)") ax.set_ylabel("joint angle (deg)") ax.set_title("joint trajectories") plt.show()
print reader import time app = ConsoleApp() view = app.createView() def spin(): polyData = vtk.vtkPolyData() reader.GetMesh(polyData) vis.updatePolyData(polyData, 'mesh') print "Number of points (a)", polyData.GetNumberOfPoints() if (polyData.GetNumberOfPoints() == 0): return polyDataPC = vtk.vtkPolyData() reader.GetPointCloud(polyDataPC) vis.updatePolyData(polyDataPC, 'output') print "Number of points (b)", polyDataPC.GetNumberOfPoints() quitTimer = TimerCallback(targetFps=5.0) quitTimer.callback = spin quitTimer.start() if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()
class ImageWidget(object): """ Wrapper for displaying vtkImageData on a director-style view. @note This is more like director's `CameraImageView` than its `ImageWidget`. """ def __init__(self, image_handler): self._name = 'Image View' self._view = PythonQt.dd.ddQVTKWidgetView() self._image_handler = image_handler self._image = vtk.vtkImageData() self._prev_attrib = None # Initialize the view. self._view.installImageInteractor() # Add actor. self._image_actor = vtk.vtkImageActor() vtk_SetInputData(self._image_actor, self._image) self._image_actor.SetVisibility(False) self._view.renderer().AddActor(self._image_actor) self._view.orientationMarkerWidget().Off() self._view.backgroundRenderer().SetBackground(0, 0, 0) self._view.backgroundRenderer().SetBackground2(0, 0, 0) self._depth_mapper = None # Add timer. self._render_timer = TimerCallback(targetFps=60, callback=self.render) self._render_timer.start() def get_widget(self): return self._view def render(self): if not self._view.isVisible(): return has_new = self._image_handler.update_image(self._image) assert isinstance(has_new, bool) if not has_new: return cur_attrib = get_vtk_image_attrib(self._image) if self._prev_attrib != cur_attrib: if self._prev_attrib is None: # Initialization. Ensure it is visible. self._image_actor.SetVisibility(True) # Fit image to view. self._on_new_image_attrib(cur_attrib) # Update. self._prev_attrib = cur_attrib if self._depth_mapper is not None: depth_range = self._get_depth_range() for i in xrange(2): value = [0.] * 6 coloring = self._depth_mapper.GetLookupTable() coloring.GetNodeValue(i, value) value[0] = depth_range[i] coloring.SetNodeValue(i, value) self._view.render() def _get_depth_range(self): lower_depth = 0 upper_depth = _max_depth if upper_depth == -1: # @note `GetScalarRange` permits non-finite values, such as `inf`. # Use a custom mechanism to get min/max. data = vtk_image_to_numpy(self._image) if data.dtype == np.float32: good = np.isfinite(data[:]) elif data.dtype == np.uint16: maxarray = np.full(data.shape, 65535) good = np.less(data[:], maxarray) else: raise RuntimeError("Unsupported depth format: {}".format( data.dtype)) if np.any(good): upper_depth = np.max(data[good]) return (lower_depth, upper_depth) def _on_new_image_attrib(self, attrib): ((w, h, num_channels), dtype) = attrib if self._image_handler.is_depth_image(): assert num_channels == 1, num_channels assert dtype in (np.uint16, np.float32), dtype # TODO(eric.cousineau): Delegate to outside of `ImageWidget`? # This is depth-image specific. # For now, just set arbitrary values. depth_range = self._get_depth_range() lower_color = (1, 1, 1) # White upper_color = (0, 0, 0) # Black nan_color = (0.5, 0.5, 1) # Light blue - No return. inf_color = (0.5, 0, 0.) # Dark red - Too far / too close. # Use `vtkColorTransferFunction` as it provides a more intuitive # interpolating interface for me (Eric) than `vtkLookupTable`, # since it permits direct specification of RGB values. coloring = vtk.vtkColorTransferFunction() coloring.AddRGBPoint(depth_range[0], *lower_color) coloring.AddRGBPoint(depth_range[1], *upper_color) coloring.SetNanColor(*nan_color) # @note `coloring.SetAboveRangeColor` doesn't seem to work? coloring.AddRGBPoint(depth_range[1] + 10000, *inf_color) coloring.SetClamping(True) coloring.SetScaleToLinear() self._depth_mapper = vtk.vtkImageMapToColors() self._depth_mapper.SetLookupTable(coloring) vtk_SetInputData(self._depth_mapper, self._image) vtk_SetInputData(self._image_actor, self._depth_mapper.GetOutput()) self._image_actor.GetMapper().SetInputConnection( self._depth_mapper.GetOutputPort()) else: # Direct connection. self._depth_mapper = None vtk_SetInputData(self._image_actor, self._image) # Must render first. self._view.render() # Fit image to view. # TODO(eric.cousineau): No idea why this is needed; it worked for # VTK 5, but no longer for VTK 6+? camera = self._view.camera() camera.ParallelProjectionOn() camera.SetFocalPoint(0, 0, 0) camera.SetPosition(0, 0, -1) camera.SetViewUp(0, -1, 0) self._view.resetCamera() image_height, image_width = get_vtk_image_shape(self._image)[:2] view_width, view_height = self._view.renderWindow().GetSize() aspect_ratio = float(view_width) / view_height parallel_scale = max(image_width / aspect_ratio, image_height) / 2.0 camera.SetParallelScale(parallel_scale)
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) result = self.currentTask() if isinstance(result, types.GeneratorType): self.generators.insert(0, result) def doWork(self): if self.generators: self.handleGenerator(self.generators[0]) else: if self.currentTask: self.completePreviousTask() if self.tasks: self.startNextTask() def handleGenerator(self, generator): try: result = generator.next() except StopIteration: self.generators.remove(generator) else: if isinstance(result, types.GeneratorType): self.generators.insert(0, result) def connectQueueStarted(self, func): return self.callbacks.connect(self.QUEUE_STARTED_SIGNAL, func) def disconnectQueueStarted(self, callbackId): self.callbacks.disconnect(callbackId) def connectQueueStopped(self, func): return self.callbacks.connect(self.QUEUE_STOPPED_SIGNAL, func) def disconnectQueueStopped(self, callbackId): self.callbacks.disconnect(callbackId) def connectTaskStarted(self, func): return self.callbacks.connect(self.TASK_STARTED_SIGNAL, func) def disconnectTaskStarted(self, callbackId): self.callbacks.disconnect(callbackId) def connectTaskEnded(self, func): return self.callbacks.connect(self.TASK_ENDED_SIGNAL, func) def disconnectTaskEnded(self, callbackId): self.callbacks.disconnect(callbackId) def connectTaskPaused(self, func): return self.callbacks.connect(self.TASK_PAUSED_SIGNAL, func) def disconnectTaskPaused(self, callbackId): self.callbacks.disconnect(callbackId) def connectTaskFailed(self, func): return self.callbacks.connect(self.TASK_FAILED_SIGNAL, func) def disconnectTaskFailed(self, callbackId): self.callbacks.disconnect(callbackId) def connectTaskException(self, func): return self.callbacks.connect(self.TASK_EXCEPTION_SIGNAL, func) def disconnectTaskException(self, callbackId): self.callbacks.disconnect(callbackId)
class ImageWidget(object): def __init__(self, imageManager, imageName, view, visible=True): self.view = view self.imageManager = imageManager self.imageName = imageName self.visible = visible self.updateUtime = 0 self.initialized = False self.imageWidget = vtk.vtkLogoWidget() imageRep = self.imageWidget.GetRepresentation() self.imageWidget.ResizableOff() self.imageWidget.SelectableOn() imageRep.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)) imageRep.SetImage(self.flip.GetOutput()) self.eventFilter = PythonQt.dd.ddPythonEventFilter() self.view.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize) self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onResizeEvent) self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def setWidgetSize(self, desiredWidth=400): image = self.imageManager.getImage(self.imageName) dims = image.GetDimensions() if 0.0 in dims: return aspectRatio = float(dims[0]) / dims[1] imageWidth, imageHeight = desiredWidth, desiredWidth / aspectRatio viewWidth, viewHeight = self.view.width, self.view.height rep = self.imageWidget.GetBorderRepresentation() rep.SetShowBorderToOff() coord = rep.GetPositionCoordinate() coord2 = rep.GetPosition2Coordinate() coord.SetCoordinateSystemToDisplay() coord2.SetCoordinateSystemToDisplay() coord.SetValue(0, viewHeight - imageHeight) coord2.SetValue(imageWidth, imageHeight) self.view.render() def onResizeEvent(self): self.setWidgetSize(400) def setImageName(self, imageName): self.imageName = imageName self.flip.SetInput(imageManager.getImage(imageName)) def setOpacity(self, opacity=1.0): self.imageWidget.GetRepresentation().GetImageProperty().SetOpacity( opacity) def hide(self): self.visible = False self.imageWidget.Off() self.view.render() def show(self): self.visible = True if self.haveImage(): self.imageWidget.On() self.view.render() def haveImage(self): image = self.imageManager.getImage(self.imageName) dims = image.GetDimensions() return 0.0 not in dims def updateView(self): if not self.visible or not self.view.isVisible(): return currentUtime = self.imageManager.updateImage(self.imageName) if currentUtime != self.updateUtime: self.updateUtime = currentUtime self.flip.Update() self.view.render() if not self.initialized and self.visible and self.haveImage(): self.show() self.setWidgetSize(400) self.initialized = True
class KorgNanoKontrol(object): def __init__(self): self.timer = TimerCallback(targetFps=10) self.timer.callback = self.tick self.stop = self.timer.stop self.reader = None self.initReader() self.inputs = { 'slider': [0, 8, True], 'dial': [16, 8, True], 'r_button': [64, 8, False], 'm_button': [48, 8, False], 's_button': [32, 8, False], 'track_left': [58, 1, False], 'track_right': [59, 1, False], 'cycle': [46, 1, False], 'marker_set': [60, 1, False], 'marker_left': [61, 1, False], 'marker_right': [62, 1, False], 'rewind': [43, 1, False], 'fastforward': [44, 1, False], 'stop': [42, 1, False], 'play': [41, 1, False], 'record': [45, 1, False], } signalNames = [] for inputName, inputDescription in self.inputs.items(): channelStart, numChannels, isContinuous = inputDescription for i in range(numChannels): channelId = '' if numChannels == 1 else '_%d' % i if isContinuous: signalNames.append('%s%s_value_changed' % (inputName, channelId)) else: signalNames.append('%s%s_pressed' % (inputName, channelId)) signalNames.append('%s%s_released' % (inputName, channelId)) self.callbacks = callbacks.CallbackRegistry(signalNames) def start(self): self.initReader() if self.reader is not None: self.timer.start() def initReader(self): if self.reader: return try: self.reader = midi.MidiReader(midi.findKorgNanoKontrol2()) except: print('midi controller not found.') self.reader = None def onMidiCommand(self, channel, value): #print channel, '%.2f' % value inputs = self.inputs for inputName, inputDescription in inputs.items(): channelStart, numChannels, isContinuous = inputDescription if channelStart <= channel < (channelStart + numChannels): if numChannels > 1: inputName = '%s_%d' % (inputName, channel - channelStart) if isContinuous: self.onContinuousInput(inputName, value) elif value == 1: self.onButtonDown(inputName) elif value == 0: self.onButtonUp(inputName) def onContinuousInput(self, name, value): #print name, '%.2f' % value self.callbacks.process(name + '_value_changed', value) def onButtonDown(self, name): #print name, 'down' self.callbacks.process(name + '_pressed') def onButtonUp(self, name): #print name, 'up' self.callbacks.process(name + '_released') def tick(self): try: messages = self.reader.getMessages() except: messages = [] if not messages: return targets = {} for message in messages: channel = message[2] value = message[3] targets[channel] = value for channel, value in targets.items(): position = value / 127.0 self.onMidiCommand(channel, position)
class CameraImageView(object): def __init__(self, imageManager, imageName, viewName=None, view=None): imageManager.addImage(imageName) self.cameraRoll = None self.imageManager = imageManager self.viewName = viewName or imageName self.imageName = imageName self.imageInitialized = False self.updateUtime = 0 self.initView(view) self.initEventFilter() def getImagePixel(self, displayPoint, restrictToImageDimensions=True): worldPoint = [0.0, 0.0, 0.0, 0.0] vtk.vtkInteractorObserver.ComputeDisplayToWorld( self.view.renderer(), displayPoint[0], displayPoint[1], 0, worldPoint) imageDimensions = self.getImage().GetDimensions() if 0.0 <= worldPoint[0] <= imageDimensions[0] and 0.0 <= worldPoint[ 1] <= imageDimensions[1] or not restrictToImageDimensions: return [worldPoint[0], worldPoint[1], 0.0] else: return None def getWorldPositionAndRay(self, imagePixel, imageUtime=None): ''' Given an XY image pixel, computes an equivalent ray in the world coordinate system using the camera to local transform at the given imageUtime. If imageUtime is None, then the utime of the most recent image is used. Returns the camera xyz position in world, and a ray unit vector. ''' if imageUtime is None: imageUtime = self.imageManager.getUtime(self.imageName) # input is pixel u,v, output is unit x,y,z in camera coordinates cameraPoint = self.imageManager.queue.unprojectPixel( self.imageName, imagePixel[0], imagePixel[1]) cameraToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform(self.imageName, 'local', imageUtime, cameraToLocal) p = np.array(cameraToLocal.TransformPoint(cameraPoint)) cameraPosition = np.array(cameraToLocal.GetPosition()) ray = p - cameraPosition ray /= np.linalg.norm(ray) return cameraPosition, ray def filterEvent(self, obj, event): if self.eventFilterEnabled and event.type( ) == QtCore.QEvent.MouseButtonDblClick: self.eventFilter.setEventHandlerResult(True) elif event.type() == QtCore.QEvent.KeyPress: if str(event.text()).lower() == 'p': self.eventFilter.setEventHandlerResult(True) elif str(event.text()).lower() == 'r': self.eventFilter.setEventHandlerResult(True) self.resetCamera() def onRubberBandPick(self, obj, event): displayPoints = self.interactorStyle.GetStartPosition( ), self.interactorStyle.GetEndPosition() imagePoints = [ vis.pickImage(point, self.view)[1] for point in displayPoints ] sendFOVRequest(self.imageName, imagePoints) def getImage(self): return self.imageManager.getImage(self.imageName) def initView(self, view): self.view = view or app.getViewManager().createView( self.viewName, 'VTK View') self.view.installImageInteractor() self.interactorStyle = self.view.renderWindow().GetInteractor( ).GetInteractorStyle() self.interactorStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPick) self.imageActor = vtk.vtkImageActor() self.imageActor.SetInput(self.getImage()) self.imageActor.SetVisibility(False) self.view.renderer().AddActor(self.imageActor) self.view.orientationMarkerWidget().Off() self.view.backgroundRenderer().SetBackground(0, 0, 0) self.view.backgroundRenderer().SetBackground2(0, 0, 0) self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def initEventFilter(self): self.eventFilter = PythonQt.dd.ddPythonEventFilter() qvtkwidget = self.view.vtkWidget() qvtkwidget.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType( QtCore.QEvent.MouseButtonDblClick) self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress) self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent) self.eventFilterEnabled = True def setCameraRoll(self, roll): self.cameraRoll = roll self.resetCamera() def resetCamera(self): camera = self.view.camera() camera.ParallelProjectionOn() camera.SetFocalPoint(0, 0, 0) camera.SetPosition(0, 0, -1) camera.SetViewUp(0, -1, 0) if self.cameraRoll is not None: camera.SetRoll(self.cameraRoll) self.view.resetCamera() self.fitImageToView() self.view.render() def fitImageToView(self): camera = self.view.camera() image = self.getImage() imageWidth, imageHeight, _ = image.GetDimensions() viewWidth, viewHeight = self.view.renderWindow().GetSize() aspectRatio = float(viewWidth) / viewHeight parallelScale = max(imageWidth / aspectRatio, imageHeight) / 2.0 camera.SetParallelScale(parallelScale) def setImageName(self, imageName): if imageName == self.imageName: return assert self.imageManager.hasImage(imageName) self.imageName = imageName self.imageInitialized = False self.updateUtime = 0 self.imageActor.SetInput(self.imageManager.getImage(self.imageName)) self.imageActor.SetVisibility(False) self.view.render() def updateView(self): if not self.view.isVisible(): return currentUtime = self.imageManager.updateImage(self.imageName) if currentUtime != self.updateUtime: self.updateUtime = currentUtime self.view.render() if not self.imageInitialized and self.imageActor.GetInput( ).GetDimensions()[0]: self.imageActor.SetVisibility(True) self.resetCamera() self.imageInitialized = True
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): ''' Set the mode of the view widget. input arg: 'continous', 'frames', 'hidden' e.g. can hide all plan playback with 'hidden' ''' self.ui.viewModeCombo.setCurrentIndex(self.ui.viewModeCombo.findText(mode)) def getPlaybackSpeed(self): s = str(self.ui.playbackSpeedCombo.currentText).replace('x', '') if '/' in s: n, d = s.split('/') return float(n)/float(d) return float(s) def getInterpolationMethod(self): return str(self.ui.interpolationCombo.currentText) def getNumberOfSamples(self): return self.ui.samplesSpinBox.value def viewModeChanged(self): viewMode = self.getViewMode() if viewMode == 'continuous': playbackVisible = True samplesVisible = False else: playbackVisible = False samplesVisible = True self.ui.samplesLabel.setEnabled(samplesVisible) self.ui.samplesSpinBox.setEnabled(samplesVisible) self.ui.playbackSpeedLabel.setVisible(playbackVisible) self.ui.playbackSpeedCombo.setVisible(playbackVisible) self.ui.playbackSlider.setEnabled(playbackVisible) self.ui.animateButton.setVisible(playbackVisible) self.ui.timeLabel.setVisible(playbackVisible) if self.plan: if self.getViewMode() == 'continuous' and self.autoPlay: self.startAnimation() else: self.ui.playbackSlider.value = 0 self.stopAnimation() self.updatePlanFrames() def playbackSpeedChanged(self): self.planPlayback.playbackSpeed = self.getPlaybackSpeed() def getPlaybackTime(self): sliderValue = self.ui.playbackSlider.value return (sliderValue / 1000.0) * self.endTime def updateTimeLabel(self): playbackTime = self.getPlaybackTime() self.ui.timeLabel.text = 'Time: %.2f s' % playbackTime def playbackSliderValueChanged(self): self.updateTimeLabel() self.showPoseAtTime(self.getPlaybackTime()) def interpolationChanged(self): methods = {'linear' : 'slinear', 'cubic spline' : 'cubic', 'pchip' : 'pchip' } self.planPlayback.interpolationMethod = methods[self.getInterpolationMethod()] self.poseInterpolator = self.planPlayback.getPoseInterpolatorFromPlan(self.plan) self.updatePlanFrames() def numberOfSamplesChanged(self): self.updatePlanFrames() def animateClicked(self): self.startAnimation() def hideClicked(self): if self.ui.hideButton.text == 'hide': self.ui.playbackFrame.setEnabled(False) self.hidePlan() self.ui.hideButton.text = 'show' self.ui.executeButton.setEnabled(False) if not self.plan: self.ui.hideButton.setEnabled(False) else: self.ui.playbackFrame.setEnabled(True) self.ui.hideButton.text = 'hide' self.ui.hideButton.setEnabled(True) self.ui.executeButton.setEnabled(True) self.viewModeChanged() self.updateButtonColor() def executeClicked(self): self.executePlan() def executePlan(self, visOnly=False, overrideInfeasibleCheck=False, overrideSupportsCheck=False): if visOnly: _, poses = self.planPlayback.getPlanPoses(self.plan) self.onPlanCommitted(self.plan) self.robotStateJointController.setPose('EST_ROBOT_STATE', poses[-1]) else: if (self.isPlanFeasible() or overrideInfeasibleCheck) and (not self.isPlanAPlanWithSupports() or overrideSupportsCheck): self.manipPlanner.commitManipPlan(self.plan) def onPlanCommitted(self, plan): self.setPlan(None) self.hideClicked() def stopClicked(self): self.stopAnimation() self.manipPlanner.sendPlanPause() def isPlanFeasible(self): plan = robotstate.asRobotPlan(self.plan) return plan is not None and (max(plan.plan_info) < 10 and min(plan.plan_info) >= 0) def getPlanInfo(self, plan): plan = robotstate.asRobotPlan(self.plan) return max(plan.plan_info) def isPlanAPlanWithSupports(self): return hasattr(self.plan, 'support_sequence') or self.manipPlanner.publishPlansWithSupports def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85/255.0, 255/255.0, 255/255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames() def showPlanFrames(self): self.planFramesObj.setProperty('Visible', True) self.robotStateModel.setProperty('Visible', False) self.playbackRobotModel.setProperty('Visible', False) def startAnimation(self): self.showPlaybackModel() self.stopAnimation() self.ui.playbackSlider.value = 0 self.animationClock.reset() self.animationTimer.start() self.updateAnimation() def stopAnimation(self): self.animationTimer.stop() def showPlaybackModel(self): self.robotStateModel.setProperty('Visible', True) self.playbackRobotModel.setProperty('Visible', True) self.playbackRobotModel.setProperty('Color Mode', 1 if self.playbackRobotModelUseTextures else 0) self.robotStateModel.setProperty('Alpha', self.robotStateModelDisplayAlpha) self.playbackRobotModel.setProperty('Alpha', self.playbackRobotModelDisplayAlpha) if self.planFramesObj: self.planFramesObj.setProperty('Visible', False) def hidePlan(self): self.stopAnimation() if self.planFramesObj: self.planFramesObj.setProperty('Visible', False) if self.playbackRobotModel: self.playbackRobotModel.setProperty('Visible', False) self.robotStateModel.setProperty('Visible', True) self.robotStateModel.setProperty('Alpha', 1.0) def showPoseAtTime(self, time): pose = self.poseInterpolator(time) self.playbackJointController.setPose('plan_playback', pose) def updateAnimation(self): tNow = self.animationClock.elapsed() * self.planPlayback.playbackSpeed if tNow > self.endTime: tNow = self.endTime sliderValue = int(1000.0 * tNow / self.endTime) self.ui.playbackSlider.blockSignals(True) self.ui.playbackSlider.value = sliderValue self.ui.playbackSlider.blockSignals(False) self.updateTimeLabel() self.showPoseAtTime(tNow) return tNow < self.endTime def updateButtonColor(self): if self.ui.executeButton.enabled and self.plan and not self.isPlanFeasible(): styleSheet = 'background-color:red' elif self.ui.executeButton.enabled and self.plan and self.isPlanAPlanWithSupports(): styleSheet = 'background-color:orange' else: styleSheet = '' self.ui.executeButton.setStyleSheet(styleSheet) def setPlan(self, plan): self.ui.playbackSlider.value = 0 self.ui.timeLabel.text = 'Time: 0.00 s' self.ui.planNameLabel.text = '' self.plan = plan self.endTime = 1.0 self.updateButtonColor() if not self.plan: return planText = 'Plan: %d. %.2f seconds' % (plan.utime, self.planPlayback.getPlanElapsedTime(plan)) self.ui.planNameLabel.text = planText self.startTime = 0.0 self.endTime = self.planPlayback.getPlanElapsedTime(plan) self.interpolationChanged() info = self.getPlanInfo(plan) app.displaySnoptInfo(info) if self.ui.hideButton.text == 'show': self.hideClicked() else: self.viewModeChanged() self.updateButtonColor() if self.autoPlay and self.widget.parent() is not None: self.widget.parent().show()
class TaskUserPanel(object): def __init__(self, windowTitle='Task Panel'): loader = QtUiTools.QUiLoader() uifile = QtCore.QFile(':/ui/ddTaskUserPanel.ui') assert uifile.open(uifile.ReadOnly) self.widget = loader.load(uifile) self.ui = WidgetDict(self.widget.children()) self.widget.setWindowTitle(windowTitle) self.manualButtons = {} self.imageViewLayout = QtGui.QHBoxLayout(self.ui.imageFrame) self._setupParams() self._setupPropertiesPanel() self._initTaskPanel() def addManualButton(self, text, callback): b = QtGui.QPushButton(text) b.connect('clicked()', callback) self.manualButtons[text] = b self.addManualWidget(b) return b def addManualSpacer(self): line = QtGui.QFrame() line.setFrameShape(QtGui.QFrame.HLine) line.setFrameShadow(QtGui.QFrame.Sunken) self.addManualWidget(line) def addManualWidget(self, widget): self.ui.manualButtonsLayout.insertWidget(self.ui.manualButtonsLayout.count()-1, widget) def initImageView(self, imageView, activateAffordanceUpdater=True): if activateAffordanceUpdater: self.affordanceUpdater = affordanceupdater.AffordanceInCameraUpdater(segmentation.affordanceManager, imageView) self.affordanceUpdater.timer.start() self.imageViewLayout.addWidget(imageView.view) def _setupParams(self): self.params = om.ObjectModelItem('Task Params') self.params.properties.connectPropertyChanged(self.onPropertyChanged) def _setupPropertiesPanel(self): l = QtGui.QVBoxLayout(self.ui.propertyFrame) l.setMargin(0) self.propertiesPanel = PythonQt.dd.ddPropertiesPanel() self.propertiesPanel.setBrowserModeToWidget() l.addWidget(self.propertiesPanel) self.panelConnector = propertyset.PropertyPanelConnector(self.params.properties, self.propertiesPanel) def onPropertyChanged(self, propertySet, propertyName): pass def getNextTasks(self): return self.taskTree.getTasks(fromSelected=True) def onContinue(self): self._activatePrompts() self.completedTasks = [] self.taskQueue.reset() for obj in self.getNextTasks(): self.taskQueue.addTask(obj.task) self.taskQueue.start() def _activatePrompts(self): rt.UserPromptTask.promptFunction = self.onTaskPrompt rt.PrintTask.printFunction = self.appendMessage def onStep(self): assert not self.taskQueue.isRunning self._activatePrompts() tasks = self.getNextTasks() if not tasks: return task = tasks[0].task self.nextStepTask = tasks[1].task if len(tasks) > 1 else None self.completedTasks = [] self.taskQueue.reset() self.taskQueue.addTask(task) self.taskQueue.start() def updateTaskButtons(self): self.ui.taskStepButton.setEnabled(not self.taskQueue.isRunning) self.ui.taskContinueButton.setEnabled(not self.taskQueue.isRunning) self.ui.taskPauseButton.setEnabled(self.taskQueue.isRunning) def onPause(self): if not self.taskQueue.isRunning: return self.nextStepTask = None currentTask = self.taskQueue.currentTask self.taskQueue.stop() if currentTask: currentTask.stop() self.appendMessage('<font color="red">paused</font>') def onQueueStarted(self, taskQueue): self.updateTaskButtons() def onQueueStopped(self, taskQueue): self.clearPrompt() self.updateTaskButtons() def onTaskStarted(self, taskQueue, task): msg = task.properties.getProperty('Name') + ' ... <font color="green">start</font>' self.appendMessage(msg) self.taskTree.selectTask(task) item = self.taskTree.findTaskItem(task) if len(self.completedTasks) and item.getProperty('Visible'): self.appendMessage('<font color="red">paused</font>') raise atq.AsyncTaskQueue.PauseException() def onTaskEnded(self, taskQueue, task): msg = task.properties.getProperty('Name') + ' ... <font color="green">end</font>' self.appendMessage(msg) self.completedTasks.append(task) if self.taskQueue.tasks: self.taskTree.selectTask(self.taskQueue.tasks[0]) elif self.nextStepTask: self.taskTree.selectTask(self.nextStepTask) #else: # self.taskTree.selectTask(self.completedTasks[0]) def onTaskFailed(self, taskQueue, task): msg = task.properties.getProperty('Name') + ' ... <font color="red">failed: %s</font>' % task.failReason self.appendMessage(msg) def onTaskPaused(self, taskQueue, task): msg = task.properties.getProperty('Name') + ' ... <font color="red">paused</font>' self.appendMessage(msg) def onTaskException(self, taskQueue, task): msg = task.properties.getProperty('Name') + ' ... <font color="red">exception:\n\n%s</font>' % traceback.format_exc() self.appendMessage(msg) def appendMessage(self, msg): if msg == self.lastStatusMessage: return self.lastStatusMessage = msg self.ui.outputConsole.append(msg.replace('\n', '<br/>')) def updateTaskStatus(self): currentTask = self.taskQueue.currentTask if not currentTask or not currentTask.statusMessage: return name = currentTask.properties.getProperty('Name') status = currentTask.statusMessage msg = name + ': ' + status self.appendMessage(msg) def clearPrompt(self): self.promptTask = None self.ui.promptLabel.text = '' self.ui.promptAcceptButton.enabled = False self.ui.promptRejectButton.enabled = False def onAcceptPrompt(self): self.promptTask.accept() self.clearPrompt() def onRejectPrompt(self): self.promptTask.reject() self.clearPrompt() def onTaskPrompt(self, task, message): self.promptTask = task self.ui.promptLabel.text = message self.ui.promptAcceptButton.enabled = True self.ui.promptRejectButton.enabled = True 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 start(self): if self.reader is None: self.reader = drc.vtkMapServerSource() self.reader.Start() TimerCallback.start(self)
class CameraView(object): def __init__(self, imageManager, view=None): self.imageManager = imageManager self.updateUtimes = {} self.robotModel = None self.sphereObjects = {} self.sphereImages = [ 'CAMERA_LEFT', 'CAMERACHEST_RIGHT', 'CAMERACHEST_LEFT' ] for name in self.sphereImages: imageManager.addImage(name) self.updateUtimes[name] = 0 self.initView(view) self.initEventFilter() self.rayCallback = rayDebug self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def onViewDoubleClicked(self, displayPoint): obj, pickedPoint = vis.findPickedObject(displayPoint, self.view) if pickedPoint is None or not obj: return imageName = obj.getProperty('Name') imageUtime = self.imageManager.getUtime(imageName) cameraToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal) utorsoToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal) p = range(3) utorsoToLocal.TransformPoint(pickedPoint, p) ray = np.array(p) - np.array(cameraToLocal.GetPosition()) ray /= np.linalg.norm(ray) if self.rayCallback: self.rayCallback(np.array(cameraToLocal.GetPosition()), ray) def filterEvent(self, obj, event): if event.type() == QtCore.QEvent.MouseButtonDblClick: self.eventFilter.setEventHandlerResult(True) self.onViewDoubleClicked(vis.mapMousePosition(obj, event)) elif event.type() == QtCore.QEvent.KeyPress: if str(event.text()).lower() == 'p': self.eventFilter.setEventHandlerResult(True) elif str(event.text()).lower() == 'r': self.eventFilter.setEventHandlerResult(True) self.resetCamera() def initEventFilter(self): self.eventFilter = PythonQt.dd.ddPythonEventFilter() qvtkwidget = self.view.vtkWidget() qvtkwidget.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType( QtCore.QEvent.MouseButtonDblClick) self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress) self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent) def initImageRotations(self, robotModel): self.robotModel = robotModel # Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie) if robotModel.getHeadLink(): tf = robotModel.getLinkFrame(robotModel.getHeadLink()) roll = transformUtils.rollPitchYawFromTransform(tf)[0] if np.isclose(np.abs(roll), np.pi, atol=1e-1): self.imageManager.setImageRotation180('CAMERA_LEFT') def initView(self, view): self.view = view or app.getViewManager().createView( 'Camera View', 'VTK View') self.renderers = [self.view.renderer()] renWin = self.view.renderWindow() renWin.SetNumberOfLayers(3) for i in [1, 2]: ren = vtk.vtkRenderer() ren.SetLayer(2) ren.SetActiveCamera(self.view.camera()) renWin.AddRenderer(ren) self.renderers.append(ren) def applyCustomBounds(): self.view.addCustomBounds([-100, 100, -100, 100, -100, 100]) self.view.connect('computeBoundsRequest(ddQVTKWidgetView*)', applyCustomBounds) app.setCameraTerrainModeEnabled(self.view, True) self.resetCamera() def resetCamera(self): self.view.camera().SetViewAngle(90) self.view.camera().SetPosition(-7.5, 0.0, 5.0) self.view.camera().SetFocalPoint(0.0, 0.0, 0.0) self.view.camera().SetViewUp(0.0, 0.0, 1.0) self.view.render() def getSphereGeometry(self, imageName): sphereObj = self.sphereObjects.get(imageName) if sphereObj: return sphereObj if not self.imageManager.getImage(imageName).GetDimensions()[0]: return None sphereResolution = 50 sphereRadii = { 'CAMERA_LEFT': 20, 'CAMERACHEST_LEFT': 20, 'CAMERACHEST_RIGHT': 20 } geometry = makeSphere(sphereRadii[imageName], sphereResolution) self.imageManager.queue.computeTextureCoords(imageName, geometry) tcoordsArrayName = 'tcoords_%s' % imageName vtkNumpy.addNumpyToVtk( geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(), 'tcoords_U') vtkNumpy.addNumpyToVtk( geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(), 'tcoords_V') geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0]) geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0]) geometry.GetPointData().SetTCoords( geometry.GetPointData().GetArray(tcoordsArrayName)) sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent='cameras') sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName)) sphereObj.actor.GetProperty().LightingOff() self.view.renderer().RemoveActor(sphereObj.actor) rendererId = 2 - self.sphereImages.index(imageName) self.renderers[rendererId].AddActor(sphereObj.actor) self.sphereObjects[imageName] = sphereObj return sphereObj def updateSphereGeometry(self): for imageName in self.sphereImages: sphereObj = self.getSphereGeometry(imageName) if not sphereObj: continue transform = vtk.vtkTransform() self.imageManager.queue.getBodyToCameraTransform( imageName, transform) sphereObj.actor.SetUserTransform(transform.GetLinearInverse()) def updateImages(self): updated = False for imageName, lastUtime in self.updateUtimes.iteritems(): currentUtime = self.imageManager.updateImage(imageName) if currentUtime != lastUtime: self.updateUtimes[imageName] = currentUtime updated = True return updated def updateView(self): if not self.view.isVisible(): return if not self.updateImages(): return self.updateSphereGeometry() self.view.render()
class ImageWidget(object): """ Wrapper for displaying vtkImageData on a director-style view. @note This is more like director's `CameraImageView` than its `ImageWidget`. """ def __init__(self, image_handler): self._name = 'Image View' self._view = PythonQt.dd.ddQVTKWidgetView() self._image_handler = image_handler self._image = vtk.vtkImageData() self._prev_attrib = None # Initialize the view. self._view.installImageInteractor() # Add actor. self._image_actor = vtk.vtkImageActor() vtk_SetInputData(self._image_actor, self._image) self._image_actor.SetVisibility(False) self._view.renderer().AddActor(self._image_actor) self._view.orientationMarkerWidget().Off() self._view.backgroundRenderer().SetBackground(0, 0, 0) self._view.backgroundRenderer().SetBackground2(0, 0, 0) self._depth_mapper = None # Add timer. self._render_timer = TimerCallback( targetFps=60, callback=self.render) self._render_timer.start() def get_widget(self): return self._view def render(self): if not self._view.isVisible(): return has_new = self._image_handler.update_image(self._image) assert isinstance(has_new, bool) if not has_new: return cur_attrib = get_vtk_image_attrib(self._image) if self._prev_attrib != cur_attrib: if self._prev_attrib is None: # Initialization. Ensure it is visible. self._image_actor.SetVisibility(True) # Fit image to view. self._on_new_image_attrib(cur_attrib) # Update. self._prev_attrib = cur_attrib if self._depth_mapper is not None: depth_range = self._get_depth_range() for i in xrange(2): value = [0.] * 6 coloring = self._depth_mapper.GetLookupTable() coloring.GetNodeValue(i, value) value[0] = depth_range[i] coloring.SetNodeValue(i, value) self._view.render() def _get_depth_range(self): lower_depth = 0 upper_depth = _max_depth if upper_depth == -1: # @note `GetScalarRange` permits non-finite values, such as `inf`. # Use a custom mechanism to get min/max. data = vtk_image_to_numpy(self._image) if data.dtype == np.float32: good = np.isfinite(data[:]) elif data.dtype == np.uint16: maxarray = np.full(data.shape, 65535) good = np.less(data[:], maxarray) else: raise RuntimeError( "Unsupported depth format: {}".format(data.dtype)) if np.any(good): upper_depth = np.max(data[good]) return (lower_depth, upper_depth) def _on_new_image_attrib(self, attrib): ((w, h, num_channels), dtype) = attrib if self._image_handler.is_depth_image(): assert num_channels == 1, num_channels assert dtype in (np.uint16, np.float32), dtype # TODO(eric.cousineau): Delegate to outside of `ImageWidget`? # This is depth-image specific. # For now, just set arbitrary values. depth_range = self._get_depth_range() lower_color = (1, 1, 1) # White upper_color = (0, 0, 0) # Black nan_color = (0.5, 0.5, 1) # Light blue - No return. inf_color = (0.5, 0, 0.) # Dark red - Too far / too close. # Use `vtkColorTransferFunction` as it provides a more intuitive # interpolating interface for me (Eric) than `vtkLookupTable`, # since it permits direct specification of RGB values. coloring = vtk.vtkColorTransferFunction() coloring.AddRGBPoint(depth_range[0], *lower_color) coloring.AddRGBPoint(depth_range[1], *upper_color) coloring.SetNanColor(*nan_color) # @note `coloring.SetAboveRangeColor` doesn't seem to work? coloring.AddRGBPoint(depth_range[1] + 10000, *inf_color) coloring.SetClamping(True) coloring.SetScaleToLinear() self._depth_mapper = vtk.vtkImageMapToColors() self._depth_mapper.SetLookupTable(coloring) vtk_SetInputData(self._depth_mapper, self._image) vtk_SetInputData(self._image_actor, self._depth_mapper.GetOutput()) self._image_actor.GetMapper().SetInputConnection( self._depth_mapper.GetOutputPort()) else: # Direct connection. self._depth_mapper = None vtk_SetInputData(self._image_actor, self._image) # Must render first. self._view.render() # Fit image to view. # TODO(eric.cousineau): No idea why this is needed; it worked for # VTK 5, but no longer for VTK 6+? camera = self._view.camera() camera.ParallelProjectionOn() camera.SetFocalPoint(0, 0, 0) camera.SetPosition(0, 0, -1) camera.SetViewUp(0, -1, 0) self._view.resetCamera() image_height, image_width = get_vtk_image_shape(self._image)[:2] view_width, view_height = self._view.renderWindow().GetSize() aspect_ratio = float(view_width) / view_height parallel_scale = max(image_width / aspect_ratio, image_height) / 2.0 camera.SetParallelScale(parallel_scale)
class CameraView(object): def __init__(self, imageManager, view=None): self.imageManager = imageManager self.updateUtimes = {} self.robotModel = None self.sphereObjects = {} self.sphereImages = ["CAMERA_LEFT", "CAMERACHEST_RIGHT", "CAMERACHEST_LEFT"] for name in self.sphereImages: imageManager.addImage(name) self.updateUtimes[name] = 0 self.initView(view) self.initEventFilter() self.rayCallback = rayDebug self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start() def onViewDoubleClicked(self, displayPoint): obj, pickedPoint = vis.findPickedObject(displayPoint, self.view) if pickedPoint is None or not obj: return imageName = obj.getProperty("Name") imageUtime = self.imageManager.getUtime(imageName) cameraToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform(imageName, "local", imageUtime, cameraToLocal) utorsoToLocal = vtk.vtkTransform() self.imageManager.queue.getTransform("utorso", "local", imageUtime, utorsoToLocal) p = range(3) utorsoToLocal.TransformPoint(pickedPoint, p) ray = np.array(p) - np.array(cameraToLocal.GetPosition()) ray /= np.linalg.norm(ray) if self.rayCallback: self.rayCallback(np.array(cameraToLocal.GetPosition()), ray) def filterEvent(self, obj, event): if event.type() == QtCore.QEvent.MouseButtonDblClick: self.eventFilter.setEventHandlerResult(True) self.onViewDoubleClicked(vis.mapMousePosition(obj, event)) elif event.type() == QtCore.QEvent.KeyPress: if str(event.text()).lower() == "p": self.eventFilter.setEventHandlerResult(True) elif str(event.text()).lower() == "r": self.eventFilter.setEventHandlerResult(True) self.resetCamera() def initEventFilter(self): self.eventFilter = PythonQt.dd.ddPythonEventFilter() qvtkwidget = self.view.vtkWidget() qvtkwidget.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick) self.eventFilter.addFilteredEventType(QtCore.QEvent.KeyPress) self.eventFilter.connect("handleEvent(QObject*, QEvent*)", self.filterEvent) def initImageRotations(self, robotModel): self.robotModel = robotModel # Rotate Multisense image/CAMERA_LEFT if the camera frame is rotated (e.g. for Valkyrie) if robotModel.getHeadLink(): tf = robotModel.getLinkFrame(robotModel.getHeadLink()) roll = transformUtils.rollPitchYawFromTransform(tf)[0] if np.isclose(np.abs(roll), np.pi, atol=1e-1): self.imageManager.setImageRotation180("CAMERA_LEFT") def initView(self, view): self.view = view or app.getViewManager().createView("Camera View", "VTK View") self.renderers = [self.view.renderer()] renWin = self.view.renderWindow() renWin.SetNumberOfLayers(3) for i in [1, 2]: ren = vtk.vtkRenderer() ren.SetLayer(2) ren.SetActiveCamera(self.view.camera()) renWin.AddRenderer(ren) self.renderers.append(ren) def applyCustomBounds(): self.view.addCustomBounds([-100, 100, -100, 100, -100, 100]) self.view.connect("computeBoundsRequest(ddQVTKWidgetView*)", applyCustomBounds) app.setCameraTerrainModeEnabled(self.view, True) self.resetCamera() def resetCamera(self): self.view.camera().SetViewAngle(90) self.view.camera().SetPosition(-7.5, 0.0, 5.0) self.view.camera().SetFocalPoint(0.0, 0.0, 0.0) self.view.camera().SetViewUp(0.0, 0.0, 1.0) self.view.render() def getSphereGeometry(self, imageName): sphereObj = self.sphereObjects.get(imageName) if sphereObj: return sphereObj if not self.imageManager.getImage(imageName).GetDimensions()[0]: return None sphereResolution = 50 sphereRadii = {"CAMERA_LEFT": 20, "CAMERACHEST_LEFT": 20, "CAMERACHEST_RIGHT": 20} geometry = makeSphere(sphereRadii[imageName], sphereResolution) self.imageManager.queue.computeTextureCoords(imageName, geometry) tcoordsArrayName = "tcoords_%s" % imageName vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 0].copy(), "tcoords_U") vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:, 1].copy(), "tcoords_V") geometry = clipRange(geometry, "tcoords_U", [0.0, 1.0]) geometry = clipRange(geometry, "tcoords_V", [0.0, 1.0]) geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName)) sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent="cameras") sphereObj.actor.SetTexture(self.imageManager.getTexture(imageName)) sphereObj.actor.GetProperty().LightingOff() self.view.renderer().RemoveActor(sphereObj.actor) rendererId = 2 - self.sphereImages.index(imageName) self.renderers[rendererId].AddActor(sphereObj.actor) self.sphereObjects[imageName] = sphereObj return sphereObj def updateSphereGeometry(self): for imageName in self.sphereImages: sphereObj = self.getSphereGeometry(imageName) if not sphereObj: continue transform = vtk.vtkTransform() self.imageManager.queue.getBodyToCameraTransform(imageName, transform) sphereObj.actor.SetUserTransform(transform.GetLinearInverse()) def updateImages(self): updated = False for imageName, lastUtime in self.updateUtimes.iteritems(): currentUtime = self.imageManager.updateImage(imageName) if currentUtime != lastUtime: self.updateUtimes[imageName] = currentUtime updated = True return updated def updateView(self): if not self.view.isVisible(): return if not self.updateImages(): return self.updateSphereGeometry() self.view.render()
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() def addTrackers(self): self.trackers = OrderedDict( [ ["Off", None], ["Position", PositionTracker], ["Position & Orientation", PositionOrientationTracker], ["Smooth Follow", SmoothFollowTracker], ["Look At", LookAtTracker], ["Orbit", OrbitTracker], ] ) def setTrackerMode(self, modeName): assert modeName in self.trackers self.trackerClass = self.trackers[modeName] self.initTracker()