diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 21973ab65..a3f7af5fe 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -67,7 +67,7 @@ set(python_files director/lcmgl.py director/lcmobjectcollection.py director/lcmoctomap.py - director/lcmcollections.py + director/lcmcollections.py director/lcmspy.py director/lcmUtils.py director/mainwindowapp.py @@ -154,6 +154,8 @@ set(python_files director/macros/segmentation_view/perspective.py director/tasks/__init__.py + director/tasks/basictasks.py + director/tasks/imagebasedaffordancefit.py director/tasks/robottasks.py director/tasks/taskmanagerwidget.py director/tasks/taskuserpanel.py diff --git a/src/python/director/doordemo.py b/src/python/director/doordemo.py index 0fca2de18..0a483f21d 100644 --- a/src/python/director/doordemo.py +++ b/src/python/director/doordemo.py @@ -28,7 +28,7 @@ from director import planplayback from director.footstepsdriver import FootstepRequestGenerator from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit +from director.tasks.imagebasedaffordancefit import ImageBasedAffordanceFit import director.tasks.robottasks as rt diff --git a/src/python/director/drilldemo.py b/src/python/director/drilldemo.py index 8b16d6a10..be46d407b 100644 --- a/src/python/director/drilldemo.py +++ b/src/python/director/drilldemo.py @@ -32,7 +32,7 @@ from director import vtkNumpy as vnp from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit +from director.tasks.imagebasedaffordancefit import ImageBasedAffordanceFit import director.tasks.robottasks as rt import director.tasks.taskmanagerwidget as tmw diff --git a/src/python/director/polarisplatformplanner.py b/src/python/director/polarisplatformplanner.py index 66d782ded..be2c32dd2 100644 --- a/src/python/director/polarisplatformplanner.py +++ b/src/python/director/polarisplatformplanner.py @@ -33,7 +33,6 @@ from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit import director.tasks.robottasks as rt import director.tasks.taskmanagerwidget as tmw diff --git a/src/python/director/surprisetask.py b/src/python/director/surprisetask.py index 4cd966445..71cda3a5d 100644 --- a/src/python/director/surprisetask.py +++ b/src/python/director/surprisetask.py @@ -31,7 +31,7 @@ from director import switchplanner from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit +from director.tasks.imagebasedaffordancefit import ImageBasedAffordanceFit import director.tasks.robottasks as rt import director.tasks.taskmanagerwidget as tmw @@ -278,7 +278,3 @@ def fitSwitchBox(self): self.fitter.imagePicker.numberOfPoints = 2 self.fitter.pointCloudSource = 'lidar' self.fitter.fitFunc = self.fitter.fitSwitchBox - - - - diff --git a/src/python/director/switchplanner.py b/src/python/director/switchplanner.py index df55ed9c5..8f8a1805b 100644 --- a/src/python/director/switchplanner.py +++ b/src/python/director/switchplanner.py @@ -30,7 +30,6 @@ from director import vtkNumpy as vnp from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit import director.tasks.robottasks as rt import director.tasks.taskmanagerwidget as tmw diff --git a/src/python/director/tasks/basictasks.py b/src/python/director/tasks/basictasks.py new file mode 100644 index 000000000..c64f3c486 --- /dev/null +++ b/src/python/director/tasks/basictasks.py @@ -0,0 +1,181 @@ +import copy +import inspect +import re + +from director import asynctaskqueue as atq +from director import propertyset +from director.simpletimer import SimpleTimer + +from PythonQt import QtCore +from PythonQt import QtGui + +def _splitCamelCase(name): + name = re.sub('(.)([A-Z][a-z]+)', r'\1 \2', name) + return re.sub('([a-z0-9])([A-Z])', r'\1 \2', name) + + +class AsyncTask(object): + ''' + AsyncTask documentation. + ''' + + def __init__(self, **kwargs): + self.statusMessage = '' + self.failReason = '' + self.properties = propertyset.PropertySet() + self.properties.addProperty('Name', _splitCamelCase(self.__class__.__name__).lower()) + + for cls in reversed(inspect.getmro(self.__class__)): + if hasattr(cls, 'getDefaultProperties'): + cls.getDefaultProperties(self.properties) + + for name, value in kwargs.iteritems(): + self.properties.setProperty(_splitCamelCase(name).capitalize(), value) + + + def __call__(self): + return self.run() + + def stop(self): + pass + + def run(self): + pass + + def fail(self, reason): + self.failReason = reason + raise atq.AsyncTaskQueue.FailException(reason) + + def copy(self): + return copy.deepcopy(self) + + +class PrintTask(AsyncTask): + ''' + Name: Print Task + Short Description: prints a string + Description: + + This task prints a message string. + ''' + + printFunction = None + + @staticmethod + def getDefaultProperties(properties): + properties.addProperty('Message', '') + + def run(self): + if self.printFunction: + self.printFunction(self.properties.message) + else: + print self.properties.message + + +class CallbackTask(AsyncTask): + + def __init__(self, callback=None, **kwargs): + AsyncTask.__init__(self, **kwargs) + self.callback = callback + + def run(self): + if self.callback: + yield self.callback() + + +class ExceptionTask(AsyncTask): + + def run(self): + raise Exception('Task exception') + + +class UserPromptTask(AsyncTask): + + promptsEnabled = True + promptFunction = None + + + @staticmethod + def getDefaultProperties(properties): + properties.addProperty('Message', 'continue?') + properties.addProperty('Always', False) + + def showUserPrompt(self): + if self.promptFunction: + self.promptFunction(self, self.properties.message) + else: + self.showDialog() + + def showDialog(self): + + self.d = QtGui.QDialog() + + buttons = QtGui.QDialogButtonBox() + buttons.addButton('Yes', QtGui.QDialogButtonBox.AcceptRole) + buttons.addButton('No', QtGui.QDialogButtonBox.RejectRole) + buttons.connect('accepted()', self.d.accept) + buttons.connect('rejected()', self.d.reject) + + l = QtGui.QVBoxLayout(self.d) + l.addWidget(QtGui.QLabel(self.properties.message)) + l.addWidget(buttons) + + self.d.setAttribute(QtCore.Qt.WA_QuitOnClose, False) + self.d.show() + self.d.raise_() + self.d.connect('accepted()', self.accept) + self.d.connect('rejected()', self.reject) + + def accept(self): + self.result = True + + def reject(self): + self.result = False + + def run(self): + + if not self.promptsEnabled and not self.properties.getProperty('Always'): + return + + self.result = None + + self.showUserPrompt() + + while self.result is None: + yield + + if not self.result: + raise atq.AsyncTaskQueue.PauseException() + + +class DelayTask(AsyncTask): + + @staticmethod + def getDefaultProperties(properties): + properties.addProperty('Delay time', 1.0, attributes=propertyset.PropertyAttributes(minimum=0.0, maximum=1e4, singleStep=0.1)) + + def run(self): + + delayTime = self.properties.getProperty('Delay time') + t = SimpleTimer() + + while True: + + elapsed = t.elapsed() + if elapsed >= delayTime: + break + + self.statusMessage = 'Waiting %.1f seconds' % (delayTime - elapsed) + yield + + +class PauseTask(AsyncTask): + + def run(self): + raise atq.AsyncTaskQueue.PauseException() + + +class QuitTask(AsyncTask): + + def run(self): + QtCore.QCoreApplication.instance().quit() diff --git a/src/python/director/tasks/imagebasedaffordancefit.py b/src/python/director/tasks/imagebasedaffordancefit.py new file mode 100644 index 000000000..f33c49462 --- /dev/null +++ b/src/python/director/tasks/imagebasedaffordancefit.py @@ -0,0 +1,42 @@ +from director import cameraview +from director.pointpicker import ImagePointPicker +from director import segmentation + +class ImageBasedAffordanceFit(object): + + def __init__(self, imageView=None, numberOfPoints=1): + + self.imageView = imageView or cameraview.CameraImageView(cameraview.imageManager, self.getImageChannel(), 'image view') + self.imagePicker = ImagePointPicker(self.imageView, numberOfPoints=numberOfPoints) + self.imagePicker.connectDoubleClickEvent(self.onImageViewDoubleClick) + self.imagePicker.annotationFunc = self.onImageAnnotation + self.imagePicker.start() + + self.pointCloudSource = 'lidar' + self.pickLineRadius = 0.05 + self.pickNearestToCamera = True + + def getImageChannel(self): + return 'CAMERA_LEFT' + + def getPointCloud(self): + assert self.pointCloudSource in ('lidar', 'stereo') + if self.pointCloudSource == 'stereo': + return segmentation.getDisparityPointCloud(decimation=1, removeOutliers=False) + else: + return segmentation.getCurrentRevolutionData() + + def onImageAnnotation(self, *points): + polyData = self.getPointCloud() + points = [self.getPointCloudLocationFromImage(p, self.imageView, polyData) for p in points] + self.fit(polyData, points) + + def getPointCloudLocationFromImage(self, imagePixel, imageView, polyData): + cameraPos, ray = imageView.getWorldPositionAndRay(imagePixel) + return segmentation.extractPointsAlongClickRay(cameraPos, ray, polyData, distanceToLineThreshold=self.pickLineRadius, nearestToCamera=self.pickNearestToCamera) + + def onImageViewDoubleClick(self, displayPoint, modifiers, imageView): + pass + + def fit(self, pointData, points): + pass diff --git a/src/python/director/tasks/robottasks.py b/src/python/director/tasks/robottasks.py index 62ce93c33..74da86d29 100644 --- a/src/python/director/tasks/robottasks.py +++ b/src/python/director/tasks/robottasks.py @@ -8,7 +8,6 @@ from director.timercallback import TimerCallback from director.simpletimer import SimpleTimer from director import ikplanner -from director import callbacks from director import robotsystem from director import transformUtils from director import affordanceitems @@ -16,11 +15,10 @@ from director.debugVis import DebugData from director import vtkAll as vtk from director import lcmUtils +from director.tasks.basictasks import (AsyncTask, PrintTask, CallbackTask, + ExceptionTask, UserPromptTask) import numpy as np import copy -import pickle -import PythonQt -from PythonQt import QtCore, QtGui import re import inspect @@ -47,145 +45,6 @@ class WalkingPlanItem(om.ObjectModelItem): pass -def _splitCamelCase(name): - name = re.sub('(.)([A-Z][a-z]+)', r'\1 \2', name) - return re.sub('([a-z0-9])([A-Z])', r'\1 \2', name) - - -class AsyncTask(object): - ''' - AsyncTask documentation. - ''' - - def __init__(self, **kwargs): - self.statusMessage = '' - self.failReason = '' - self.properties = propertyset.PropertySet() - self.properties.addProperty('Name', _splitCamelCase(self.__class__.__name__).lower()) - - for cls in reversed(inspect.getmro(self.__class__)): - if hasattr(cls, 'getDefaultProperties'): - cls.getDefaultProperties(self.properties) - - for name, value in kwargs.iteritems(): - self.properties.setProperty(_splitCamelCase(name).capitalize(), value) - - - def __call__(self): - return self.run() - - def stop(self): - pass - - def run(self): - pass - - def fail(self, reason): - self.failReason = reason - raise atq.AsyncTaskQueue.FailException(reason) - - def copy(self): - return copy.deepcopy(self) - - -class PrintTask(AsyncTask): - ''' - Name: Print Task - Short Description: prints a string - Description: - - This task prints a message string. - ''' - - printFunction = None - - @staticmethod - def getDefaultProperties(properties): - properties.addProperty('Message', '') - - def run(self): - if self.printFunction: - self.printFunction(self.properties.message) - else: - print self.properties.message - - -class CallbackTask(AsyncTask): - - def __init__(self, callback=None, **kwargs): - AsyncTask.__init__(self, **kwargs) - self.callback = callback - - def run(self): - if self.callback: - yield self.callback() - - -class ExceptionTask(AsyncTask): - - - def run(self): - raise Exception('Task exception') - - -class UserPromptTask(AsyncTask): - - promptsEnabled = True - promptFunction = None - - - @staticmethod - def getDefaultProperties(properties): - properties.addProperty('Message', 'continue?') - properties.addProperty('Always', False) - - def showUserPrompt(self): - if self.promptFunction: - self.promptFunction(self, self.properties.message) - else: - self.showDialog() - - def showDialog(self): - - self.d = QtGui.QDialog() - - buttons = QtGui.QDialogButtonBox() - buttons.addButton('Yes', QtGui.QDialogButtonBox.AcceptRole) - buttons.addButton('No', QtGui.QDialogButtonBox.RejectRole) - buttons.connect('accepted()', self.d.accept) - buttons.connect('rejected()', self.d.reject) - - l = QtGui.QVBoxLayout(self.d) - l.addWidget(QtGui.QLabel(self.properties.message)) - l.addWidget(buttons) - - self.d.setAttribute(QtCore.Qt.WA_QuitOnClose, False) - self.d.show() - self.d.raise_() - self.d.connect('accepted()', self.accept) - self.d.connect('rejected()', self.reject) - - def accept(self): - self.result = True - - def reject(self): - self.result = False - - def run(self): - - if not self.promptsEnabled and not self.properties.getProperty('Always'): - return - - self.result = None - - self.showUserPrompt() - - while self.result is None: - yield - - if not self.result: - raise atq.AsyncTaskQueue.PauseException() - class CheckPlanInfo(UserPromptTask): @staticmethod @@ -200,39 +59,6 @@ def run(self): return UserPromptTask.run(self) -class DelayTask(AsyncTask): - - @staticmethod - def getDefaultProperties(properties): - properties.addProperty('Delay time', 1.0, attributes=propertyset.PropertyAttributes(minimum=0.0, maximum=1e4, singleStep=0.1)) - - def run(self): - - delayTime = self.properties.getProperty('Delay time') - t = SimpleTimer() - - while True: - - elapsed = t.elapsed() - if elapsed >= delayTime: - break - - self.statusMessage = 'Waiting %.1f seconds' % (delayTime - elapsed) - yield - - -class PauseTask(AsyncTask): - - def run(self): - raise atq.AsyncTaskQueue.PauseException() - - -class QuitTask(AsyncTask): - - def run(self): - QtCore.QCoreApplication.instance().quit() - - class WaitForMultisenseLidar(AsyncTask): @staticmethod @@ -1200,4 +1026,3 @@ def run(self): plan = self.constraintSet.runIkTraj() _addPlanItem(plan, '%s gaze plan' % gazeTargetFrame.getProperty('Name'), ManipulationPlanItem) - diff --git a/src/python/director/tasks/taskmanagerwidget.py b/src/python/director/tasks/taskmanagerwidget.py index 64e841061..e1f418f10 100644 --- a/src/python/director/tasks/taskmanagerwidget.py +++ b/src/python/director/tasks/taskmanagerwidget.py @@ -1,7 +1,14 @@ -from director.tasks.robottasks import * +import pickle +import re + +from director import callbacks +import director.objectmodel as om from director.tasks.descriptions import loadTaskDescription import director.applogic as app +import PythonQt +from PythonQt import QtGui + def _splitCamelCase(name): name = re.sub('(.)([A-Z][a-z]+)', r'\1 \2', name) return re.sub('([a-z0-9])([A-Z])', r'\1 \2', name) @@ -442,4 +449,3 @@ def init(): dock = app.addWidgetToDock(panel.taskLibraryWidget.widget) dock.hide() return panel - diff --git a/src/python/director/tasks/taskuserpanel.py b/src/python/director/tasks/taskuserpanel.py index 7f8009dca..456a32ca9 100644 --- a/src/python/director/tasks/taskuserpanel.py +++ b/src/python/director/tasks/taskuserpanel.py @@ -1,20 +1,8 @@ -from director import transformUtils from director.timercallback import TimerCallback from director import objectmodel as om -from director import visualization as vis -from director import applogic as app -from director.debugVis import DebugData -from director import ioUtils -from director.simpletimer import SimpleTimer -from director.utime import getUtime -from director.pointpicker import ImagePointPicker -from director import affordanceitems -from director import affordanceupdater -from director import cameraview -from director import segmentation from director import propertyset from director import asynctaskqueue as atq -import director.tasks.robottasks as rt +import director.tasks.basictasks as basictasks import director.tasks.taskmanagerwidget as tmw import numpy as np @@ -77,6 +65,7 @@ def addManualWidget(self, widget): def initImageView(self, imageView, activateAffordanceUpdater=True): if activateAffordanceUpdater: + from director import affordanceupdater self.affordanceUpdater = affordanceupdater.AffordanceInCameraUpdater(segmentation.affordanceManager, imageView) self.affordanceUpdater.timer.start() self.imageViewLayout.addWidget(imageView.view) @@ -114,8 +103,8 @@ def onContinue(self): def _activatePrompts(self): - rt.UserPromptTask.promptFunction = self.onTaskPrompt - rt.PrintTask.printFunction = self.appendMessage + basictasks.UserPromptTask.promptFunction = self.onTaskPrompt + basictasks.PrintTask.printFunction = self.appendMessage def onStep(self): @@ -267,43 +256,3 @@ def _initTaskPanel(self): self.ui.promptRejectButton.connect('clicked()', self.onRejectPrompt) self.clearPrompt() self.updateTaskButtons() - - -class ImageBasedAffordanceFit(object): - - def __init__(self, imageView=None, numberOfPoints=1): - - self.imageView = imageView or cameraview.CameraImageView(cameraview.imageManager, self.getImageChannel(), 'image view') - self.imagePicker = ImagePointPicker(self.imageView, numberOfPoints=numberOfPoints) - self.imagePicker.connectDoubleClickEvent(self.onImageViewDoubleClick) - self.imagePicker.annotationFunc = self.onImageAnnotation - self.imagePicker.start() - - self.pointCloudSource = 'lidar' - self.pickLineRadius = 0.05 - self.pickNearestToCamera = True - - def getImageChannel(self): - return 'CAMERA_LEFT' - - def getPointCloud(self): - assert self.pointCloudSource in ('lidar', 'stereo') - if self.pointCloudSource == 'stereo': - return segmentation.getDisparityPointCloud(decimation=1, removeOutliers=False) - else: - return segmentation.getCurrentRevolutionData() - - def onImageAnnotation(self, *points): - polyData = self.getPointCloud() - points = [self.getPointCloudLocationFromImage(p, self.imageView, polyData) for p in points] - self.fit(polyData, points) - - def getPointCloudLocationFromImage(self, imagePixel, imageView, polyData): - cameraPos, ray = imageView.getWorldPositionAndRay(imagePixel) - return segmentation.extractPointsAlongClickRay(cameraPos, ray, polyData, distanceToLineThreshold=self.pickLineRadius, nearestToCamera=self.pickNearestToCamera) - - def onImageViewDoubleClick(self, displayPoint, modifiers, imageView): - pass - - def fit(self, pointData, points): - pass diff --git a/src/python/director/terraintask.py b/src/python/director/terraintask.py index 68a4f4b6b..b4ca5e285 100644 --- a/src/python/director/terraintask.py +++ b/src/python/director/terraintask.py @@ -37,7 +37,7 @@ import director.terrain from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit +from director.tasks.imagebasedaffordancefit import ImageBasedAffordanceFit import director.tasks.robottasks as rt import director.tasks.taskmanagerwidget as tmw @@ -1545,7 +1545,3 @@ def addApproach(): message='Please enable recovery.'), parent=finish) addTask(rt.UserPromptTask(name='Enable pressure control', message='Please enable pressure control.'), parent=finish) - - - - diff --git a/src/python/director/valvedemo.py b/src/python/director/valvedemo.py index ee9336229..ed1c55a85 100644 --- a/src/python/director/valvedemo.py +++ b/src/python/director/valvedemo.py @@ -13,7 +13,7 @@ from director import robotstate from director import segmentation from director.tasks.taskuserpanel import TaskUserPanel -from director.tasks.taskuserpanel import ImageBasedAffordanceFit +from director.tasks.imagebasedaffordancefit import ImageBasedAffordanceFit from director.uuidutil import newUUID from director import ioUtils from director import ikplanner