Python ConsoleApp.showObjectModel方法代码示例

# 需要导入模块: from director.consoleapp import ConsoleApp [as 别名]
# 或者: from director.consoleapp.ConsoleApp import showObjectModel [as 别名]
from director import roboturdf

app = ConsoleApp()

# create a view
view = app.createView()
segmentation._defaultSegmentationView = view

robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# Move robot to near to table:
robotStateJointController.q [5] = math.radians(120)
robotStateJointController.q[0] = -1.5
robotStateJointController.q[1] = 2
robotStateJointController.q[2] = 0.95
robotStateJointController.push()

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-and-bin-scene.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot')

#segmentation.segmentTableScene(polyData, [-1.58661389,  2.91242337,  0.79958105] )
segmentation.segmentTableSceneClusters(polyData, [-1.58661389,  2.91242337,  0.79958105], clusterInXY=True )

if app.getTestingInteractiveEnabled():
    view.show()
    app.showObjectModel()
    app.start()

# 需要导入模块: from director.consoleapp import ConsoleApp [as 别名]
# 或者: from director.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)

        self.config = drcargs.getDirectorConfig()
        jointGroups = self.config['teleopJointGroups']
        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
#.........这里部分代码省略.........