Python ConsoleApp.showObjectModel方法代码示例

# 需要导入模块: from ddapp.consoleapp import ConsoleApp [as 别名]
# 或者: from ddapp.consoleapp.ConsoleApp import showObjectModel [as 别名]
robotStateJointController.push()
'''

groundHeight = 0.0
viewFrame = segmentation.transformUtils.frameFromPositionAndRPY([1, -1, groundHeight + 1.5], [0, 0, -35])

segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame)

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud( polyData )
vis.showPolyData(polyData, 'pointcloud snapshot')

# Use only returns near the robot:
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x', [0, 1.3])

segmentation.segmentTableThenFindDrills(polyData, [1.2864902,  -0.93351376,  1.10208917])

if app.getTestingInteractiveEnabled():

    v = applogic.getCurrentView()
    v.camera().SetPosition([3,3,3])
    v.camera().SetFocalPoint([0,0,0])

    view.show()
    app.showObjectModel()
    app.start()

# 需要导入模块: from ddapp.consoleapp import ConsoleApp [as 别名]
# 或者: from ddapp.consoleapp.ConsoleApp import showObjectModel [as 别名]
class AtlasCommandPanel(object):

    def __init__(self):

        self.app = ConsoleApp()
        self.view = self.app.createView()
        self.robotSystem = robotsystem.create(self.view)

        jointGroups = getJointGroups()
        self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups)
        self.jointCommandPanel = JointCommandPanel(self.robotSystem)

        self.jointCommandPanel.ui.speedSpinBox.setEnabled(False)

        self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms)
        self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs)
        self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders)
        self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged)
        self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged)

        self.widget = QtGui.QWidget()

        gl = QtGui.QGridLayout(self.widget)
        gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan
        gl.addWidget(self.view, 0, 1, 1, 1)
        gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1)
        gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1)
        gl.setRowStretch(0,1)
        gl.setColumnStretch(1,1)

        #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan)
        lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
        lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)


    def onSingleJointPositionGoal(self, msg):
        jointPositionGoal = msg.joint_position
        jointName = msg.joint_name
        allowedJointNames = ['l_leg_aky','l_arm_lwy']

        if not (jointName in allowedJointNames):
            print 'Position goals are not allowed for this joint'
            print 'ignoring this position goal'
            print 'use the sliders instead'
            return

        if (jointName == 'l_arm_lwy') and (not self.jointCommandPanel.steeringControlEnabled):
            print 'Steering control not enabled'
            print 'ignoring steering command'
            return

        if (jointName == 'l_leg_aky') and (not self.jointCommandPanel.throttleControlEnabled):
            print 'Throttle control not enabled'
            print 'ignoring throttle command'
            return
            
        jointIdx = self.jointTeleopPanel.toJointIndex(joint_name)
        self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal
        self.jointTeleopPanel.updateSliders()
        self.jointTeleopPanel.sliderChanged(jointName)

            
    def onRobotPlan(self, msg):
        playback = planplayback.PlanPlayback()
        playback.interpolationMethod = 'pchip'
        poseTimes, poses = playback.getPlanPoses(msg)
        f = playback.getPoseInterpolator(poseTimes, poses)

        jointController = self.robotSystem.teleopJointController

        timer = simpletimer.SimpleTimer()

        def setPose(pose):
            jointController.setPose('plan_playback', pose)
            self.jointTeleopPanel.endPose = pose
            self.jointTeleopPanel.updateSliders()
            commandStream.setGoalPose(pose)

        def updateAnimation():

            tNow = timer.elapsed()

            if tNow > poseTimes[-1]:
                pose = poses[-1]
                setPose(pose)
                return False

            pose = f(tNow)
            setPose(pose)


        self.animationTimer = TimerCallback()
        self.animationTimer.targetFps = 60
        self.animationTimer.callback = updateAnimation
        self.animationTimer.start()


    def mirrorJointsChanged(self):
        self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked
        self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked
#.........这里部分代码省略.........