@@ -0,0 +1,2871 @@
+from __future__ import division%0A%0Aimport numpy as np%0A%0Aimport ddapp.objectmodel as om%0Aimport ddapp.visualization as vis%0Afrom ddapp import transformUtils%0Afrom ddapp.debugVis import DebugData%0Afrom PythonQt import QtGui%0A%0Aclass TerrainRegionItem(vis.PolyDataItem):%0A    def __init__(self, name, view, seed_pose, terrain_segmentation):%0A%0A        vis.PolyDataItem.__init__(self, name, None, view)%0A%0A        self.frameObj = vis.FrameItem('region seed', seed_pose, view)%0A        om.addToObjectModel(self.frameObj, om.getOrCreateContainer('IRIS region seeds'))%0A        self.frameObj.setProperty('Scale', 0.25)%0A        self.frameObj.setProperty('Visible', True)%0A        self.frameObj.setProperty('Edit', True)%0A        self.frameObj.connectFrameModified(self.onFrameModified)%0A        self.terrain_segmentation = terrain_segmentation%0A        self.safe_region = None%0A        self.addProperty('Visible', True)%0A        self.addProperty('Enabled for Walking', True)%0A        self.addProperty('Edit', True)%0A        self.addProperty('Color', QtGui.QColor(200,200,20))%0A        self.addProperty('Alpha', 1.0)%0A        self.onFrameModified(self.frameObj)%0A        self.setProperty('Color', QtGui.QColor(200,200,20))%0A%0A    def onFrameModified(self, frame):%0A        pos, wxyz = transformUtils.poseFromTransform(frame.transform)%0A        rpy = transformUtils.rollPitchYawFromTransform(frame.transform)%0A        pose = np.hstack((pos, rpy))%0A        self.safe_region = self.terrain_segmentation.findSafeRegion(pose, iter_limit=2)%0A        debug = DebugData()%0A        xy_verts = self.safe_region.xy_polytope()%0A        for j in range(xy_verts.shape%5B1%5D):%0A            z = pos%5B2%5D%0A            p1 = np.hstack((xy_verts%5B:,j%5D, z))%0A            if j %3C xy_verts.shape%5B1%5D - 1:%0A                p2 = np.hstack((xy_verts%5B:,j+1%5D, z))%0A            else:%0A                p2 = np.hstack((xy_verts%5B:,0%5D, z))%0A            debug.addLine(p1, p2, color=%5B.8,.8,.2%5D)%0A        debug.addSphere(pos, color=%5B.8, .8, .2%5D)%0A        self.setPolyData(debug.getPolyData())%0A%0A    def _onPropertyChanged(self, propertySet, propertyName):%0A        vis.PolyDataItem._onPropertyChanged(self, propertySet, propertyName)%0A%0A        if propertyName == 'Enabled for Walking':%0A            if self.getProperty('Enabled for Walking'):%0A                self.setProperty('Alpha', 1.0)%0A            else:%0A                self.setProperty('Alpha', 0.25)%0A        elif propertyName in %5B'Visible', 'Edit'%5D:%0A            if self.getProperty('Visible') and self.getProperty('Edit'):%0A                self.frameObj.setProperty('Visible', True)%0A                self.frameObj.setProperty('Edit', True)%0A            else:%0A                self.frameObj.setProperty('Visible', False)%0A                self.frameObj.setProperty('Edit', False)%0A%0A%0A    def onRemoveFromObjectModel(self):%0A        om.removeFromObjectModel(self.frameObj)%0A        vis.PolyDataItem.onRemoveFromObjectModel(self)%0A

add terrain item class