def sendWorkspaceDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_WORKSPACE_C, repeatTime)
def sendSceneDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_SCENE, repeatTime)
def sendFusedDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.FUSED_DEPTH, repeatTime)
def sendFusedHeightRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.FUSED_HEIGHT, repeatTime)
handJoints = []
if drcargs.args().directorConfigFile.find('atlas') != -1:
handJoints = roboturdf.getRobotiqJoints() + ['neck_ay']
else:
for handModel in ikPlanner.handModels:
handJoints += handModel.handModel.model.getJointNames()
# filter base joints out
handJoints = [ joint for joint in handJoints if joint.find('base')==-1 ]
teleopJointPropagator = JointPropagator(robotStateModel, teleopRobotModel, handJoints)
playbackJointPropagator = JointPropagator(robotStateModel, playbackRobotModel, handJoints)
def doPropagation(model=None):
if teleopRobotModel.getProperty('Visible'):
teleopJointPropagator.doPropagation()
if playbackRobotModel.getProperty('Visible'):
playbackJointPropagator.doPropagation()
robotStateModel.connectModelChanged(doPropagation)
sendDataRequest(lcmdrc.data_request_t.HEIGHT_MAP_SCENE, repeatTime)
def sendWorkspaceDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_WORKSPACE_C, repeatTime)
def sendSceneDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.DEPTH_MAP_SCENE, repeatTime)
def sendFusedDepthRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.FUSED_DEPTH, repeatTime)
def sendFusedHeightRequest(repeatTime=0.0):
sendDataRequest(lcmdrc.data_request_t.FUSED_HEIGHT, repeatTime)
teleopJointPropagator = JointPropagator(robotStateModel, teleopRobotModel, roboturdf.getRobotiqJoints() + ['neck_ay'])
playbackJointPropagator = JointPropagator(robotStateModel, playbackRobotModel, roboturdf.getRobotiqJoints())
def doPropagation(model=None):
if teleopRobotModel.getProperty('Visible'):
teleopJointPropagator.doPropagation()
if playbackRobotModel.getProperty('Visible'):
playbackJointPropagator.doPropagation()
robotStateModel.connectModelChanged(doPropagation)
#app.addToolbarMacro('scene height', sendSceneHeightRequest)
#app.addToolbarMacro('scene depth', sendSceneDepthRequest)
#app.addToolbarMacro('stereo height', sendFusedHeightRequest)
#app.addToolbarMacro('stereo depth', sendFusedDepthRequest)
jointLimitChecker = teleoppanel.JointLimitChecker(robotStateModel, robotStateJointController)
