Python debugVis.DebugData类代码示例

def buildRobot():

    d = DebugData()
    d.addCone((0,0,0), (1,0,0), height=0.2, radius=0.1)
    obj = vis.showPolyData(d.getPolyData(), 'robot')
    frame = vis.addChildFrame(obj)
    return obj

def main():
    global app, view, nav_data

    nav_data = np.array([[0, 0, 0]])

    lcmUtils.addSubscriber(".*_NAV$", node_nav_t, handleNavData)

    app = ConsoleApp()

    app.setupGlobals(globals())
    app.showPythonConsole()

    view = app.createView()
    view.show()

    global d
    d = DebugData()
    d.addLine([0,0,0], [1,0,0], color=[0,1,0])
    d.addSphere((0,0,0), radius=0.02, color=[1,0,0])

    #vis.showPolyData(d.getPolyData(), 'my debug geometry', colorByName='RGB255')

    startSwarmVisualization()

    app.start()

    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 drawCenterOfMass(model):
    stanceFrame = footstepsDriver.getFeetMidPoint(model)
    com = list(model.model.getCenterOfMass())
    com[2] = stanceFrame.GetPosition()[2]
    d = DebugData()
    d.addSphere(com, radius=0.015)
    obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)

    def drawContactPts(self, obj, footstep, **kwargs):
        leftPoints, rightPoints = FootstepsDriver.getContactPts(footstep.params.support_contact_groups)
        contact_pts = rightPoints if footstep.is_right_foot else leftPoints

        d = DebugData()
        for pt in contact_pts:
            d.addSphere(pt, radius=0.01)
        d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
        d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())

def newMesh():

    d = DebugData()
    d.addArrow((0,0,0), (0,0,0.3))
    pd = d.getPolyData()
    meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)

    desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
    return affordanceFromDescription(desc)

 def updateGeometryFromProperties(self):
     radius = self.getProperty('Radius')
     circlePoints = np.linspace(0, 2*np.pi, self.getProperty('Segments')+1)
     spokes = [(0.0, np.sin(x), np.cos(x)) for x in circlePoints]
     spokes = [radius*np.array(x)/np.linalg.norm(x) for x in spokes]
     d = DebugData()
     for a, b in zip(spokes, spokes[1:]):
         d.addCapsule(center=(a+b)/2.0, axis=(b-a), length=np.linalg.norm(b-a), radius=self.getProperty('Tube Radius'))
     self.setPolyData(d.getPolyData())

    def drawTargetPath(self):
        path = DebugData()
        for i in range(1,len(self.targetPath)):
          p0 = self.targetPath[i-1].GetPosition()
          p1 = self.targetPath[i].GetPosition()
          path.addLine ( np.array( p0 ) , np.array(  p1 ), radius= 0.005)

        pathMesh = path.getPolyData()
        self.targetPathMesh = vis.showPolyData(pathMesh, 'target frame desired path', color=[0.0, 0.3, 1.0], parent=self.targetAffordance, alpha=0.6)
        self.targetPathMesh.actor.SetUserTransform(self.targetFrame)

    def onSpawnMesh(self):

        d = DebugData()
        d.addArrow((0,0,0), (0,0,0.3))
        pd = d.getPolyData()
        meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)

        pose = transformUtils.poseFromTransform(self.getSpawnFrame())
        desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
        return self.affordanceManager.newAffordanceFromDescription(desc)

def showBoardDebug(affs=None):
    referenceFrame = vtk.vtkTransform()
    referenceFrame.Translate(0, 0, 5.0)
    affs = affs or om.getObjects()
    for obj in affs:
        if isinstance(obj, BlockAffordanceItem):
            d = DebugData()
            d.addSphere(computeClosestCorner(obj, referenceFrame), radius=0.015)
            showPolyData(d.getPolyData(), 'closest corner', parent='board debug', visible=True)
            showFrame(computeGroundFrame(obj, referenceFrame), 'ground frame', parent='board debug', visible=True)

 def drawContactPts(self, obj, footstep, **kwargs):
     if footstep.is_right_foot:
         _, contact_pts = FootstepsDriver.getContactPts()
     else:
         contact_pts, _ = FootstepsDriver.getContactPts()
     d = DebugData()
     for pt in contact_pts:
         d.addSphere(pt, radius=0.01)
     d_obj = vis.showPolyData(d.getPolyData(), "contact points", parent=obj, **kwargs)
     d_obj.actor.SetUserTransform(obj.actor.GetUserTransform())

    def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):

        d = DebugData()
        self.uid = uid
        vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
        self.transform = seed_pose
        d.addSphere((0,0,0), radius=0.02)
        self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
        self.seedObj.actor.SetUserTransform(self.transform)
        self.frameObj = vis.showFrame(self.transform, 'region seed frame',
                                      scale=0.2,
                                      visible=False,
                                      parent=self.seedObj)
        self.frameObj.setProperty('Edit', True)

        self.frameObj.widget.HandleRotationEnabledOff()

        terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
        if terrain:
            rep = self.frameObj.widget.GetRepresentation()
            rep.SetTranslateAxisEnabled(2, False)
            rep.SetRotateAxisEnabled(0, False)
            rep.SetRotateAxisEnabled(1, False)

            pos = np.array(self.frameObj.transform.GetPosition())
            polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
            if polyData.GetNumberOfPoints():
                polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
                polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
                if polyData.GetNumberOfPoints():
                    pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
                    self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))

            self.placer = PlacerWidget(view, self.seedObj, terrain)
            self.placer.start()
        else:
            self.frameObj.setProperty('Edit', True)
            self.frameObj.setProperty('Visible', True)


        self.driver = irisDriver
        self.safe_region = None
        self.addProperty('Visible', True)
        self.addProperty('Enabled for Walking', True)
        self.addProperty('Alpha', 1.0)
        self.addProperty('Color', QtGui.QColor(200,200,20))

        self.frameObj.connectFrameModified(self.onFrameModified)
        if existing_region is None:
            self.onFrameModified(self.frameObj)
        else:
            self.setRegion(existing_region)

        self.setProperty('Alpha', 0.5)
        self.setProperty('Color', QtGui.QColor(220,220,220))

    def buildWarehouseWorld(percentObsDensity, nonRandom=False, circleRadius=0.1, scale=None, randomSeed=5,
                         obstaclesInnerFraction=1.0):

        if nonRandom:
            np.random.seed(randomSeed)

        d = DebugData()
        worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Warehouse")

        numObstacles = 8
 
        obsLength = 2.0

        worldLength = worldXmax - worldXmin

        print worldXmin
        print worldXmax

        obstacleZone = [worldXmin + 0.2 * worldLength, worldXmax - 0.2 * worldLength ]

        print obstacleZone

        obstacleLength = obstacleZone[1] - obstacleZone[0]

        incrementSize = obstacleLength * 1.0 / numObstacles

        print incrementSize

        leftOrRight = -1.0
        for i in xrange(numObstacles):
            
            firstX = obstacleZone[0] + incrementSize * i
            leftOrRight = leftOrRight * -1.0

            firstEndpt = (firstX, leftOrRight * worldYmax,0.0)
            secondEndpt = (firstX, 0.0, 0.0)

            #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn())
            d.addLine(firstEndpt, secondEndpt, radius=circleRadius)

        obj = vis.showPolyData(d.getPolyData(), 'world')

        world = World()
        world.visObj = obj
        world.Xmax = worldXmax
        world.Xmin = worldXmin
        world.Ymax = worldYmax
        world.Ymin = worldYmin
        world.numObstacles = numObstacles
        world.percentObsDensity = percentObsDensity

        return world

    def updateCursor(self, displayPoint):

        center = self.displayPointToImagePoint(displayPoint, restrictToImageDimensions=False)
        center = np.array(center)

        d = DebugData()
        d.addLine(center + [0, -3000, 0], center + [0, 3000, 0])
        d.addLine(center + [-3000, 0, 0], center + [3000, 0, 0])
        self.cursorObj = vis.updatePolyData(d.getPolyData(), 'cursor', alpha=0.5, view=self.view)
        self.cursorObj.addToView(self.view)
        self.cursorObj.actor.SetUseBounds(False)
        self.cursorObj.actor.SetPickable(False)
        self.view.render()

def loadFootMeshes():
    meshes = []
    for i in  range(0,2):
        d = DebugData()

        for footMeshFile in _footMeshFiles[i]:
          d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))

        t = vtk.vtkTransform()
        t.Scale(0.98, 0.98, 0.98)
        pd = filterUtils.transformPolyData(d.getPolyData(), t)
        meshes.append(pd)
    return meshes