Task panel without segmentation

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
@@ -155,6 +155,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

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

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

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

src/python/director/pydrakeik.py

@@ -328,6 +328,14 @@ def handleQuatConstraint(self, c, fields):
         qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId, quat, tolerance, tspan)
         return qc
 
+    def handleWorldFixedOrientConstraint(self, c, fields):
+
+        bodyId = self.bodyNameToId[c.linkName]
+        tspan = np.asarray(c.tspan, dtype=float)
+
+        wc = pydrakeik.WorldFixedOrientConstraint(self.rigidBodyTree, bodyId, tspan)
+        return wc
+
     def handleWorldGazeDirConstraint(self, c, fields):
 
         bodyId = self.bodyNameToId[c.linkName]
@@ -341,6 +349,24 @@ def handleWorldGazeDirConstraint(self, c, fields):
         wc = pydrakeik.WorldGazeDirConstraint(self.rigidBodyTree, bodyId, bodyAxis, worldAxis, coneThreshold, tspan)
         return wc
 
+    def handleWorldGazeOrientConstraint(self, c, fields):
+
+        bodyId = self.bodyNameToId[c.linkName]
+        tspan = np.asarray(c.tspan, dtype=float)
+        axis = np.asarray(c.axis, dtype=float)
+        coneThreshold = c.coneThreshold
+        threshold = c.threshold
+
+        if isinstance(c.quaternion, vtk.vtkTransform):
+            quat = transformUtils.getNumpyFromTransform(c.quaternion)
+        else:
+            quat = np.asarray(c.quaternion, dtype=float)
+        if quat.shape == (4,4):
+            quat = transformUtils.transformations.quaternion_from_matrix(quat)
+
+        wc = pydrakeik.WorldGazeOrientConstraint(self.rigidBodyTree, bodyId, axis, quat, coneThreshold, threshold, tspan)
+        return wc
+
     def handlePostureConstraint(self, c, fields):
 
         tspan = np.asarray(c.tspan, dtype=float)
@@ -390,7 +416,9 @@ def makeConstraints(self, fields):
             ikconstraints.PositionConstraint : self.handlePositionConstraint,
             ikconstraints.FixedLinkFromRobotPoseConstraint : self.handleFixedLinkFromRobotPoseConstraint,
             ikconstraints.QuatConstraint : self.handleQuatConstraint,
+            ikconstraints.WorldFixedOrientConstraint : self.handleWorldFixedOrientConstraint,
             ikconstraints.WorldGazeDirConstraint : self.handleWorldGazeDirConstraint,
+            ikconstraints.WorldGazeOrientConstraint : self.handleWorldGazeOrientConstraint,
             ikconstraints.PostureConstraint : self.handlePostureConstraint,
             ikconstraints.QuasiStaticConstraint : self.handleQuasiStaticConstraint,
             }

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
-
-
-
-

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

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', '<empty 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()

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