def GetStageState (self):
while not self.bInitializedServices:
rospy.sleep(0.5)
rvStageState = SrvFrameStateResponse()
rvStageState.state = None
if (self.jointstate1 is not None):
if (self.stateVisual is not None):
rvStageState.state = self.stateVisual
else:
rvStageState.state = self.stateKinematic
return rvStageState.state
def GetStageState (self):
while not self.initializedServices:
rospy.sleep(0.5)
rvStageState = SrvFrameStateResponse()
if (self.jointstate1 is not None):
rvStageState.state = self.stateVisual
return rvStageState.state
def SetStageState_callback(self, reqStageState):
while not self.initialized:
rospy.sleep(0.5)
(self.stateRef, isInArena) = self.TransformStateToFrame('Stage', reqStageState.state)
rvStageState = SrvFrameStateResponse()
rvStageState.state = reqStageState.state
return rvStageState
def SetStageStateRef_callback(self, reqStageState):
while not self.bInitialized:
rospy.sleep(0.5)
Jinv = self.JacobianInv(self.angposMech)
(self.stateRef, isInArena) = self.TransformStateToFrame('Stage', reqStageState.state)
self.angposRef = self.Get1FromPt(self.stateRef.pose.position)
self.angvelRef = Jinv.dot(np.array([[self.stateRef.velocity.linear.x],[self.stateRef.velocity.linear.y]]))
rvStageState = SrvFrameStateResponse()
rvStageState.state = reqStageState.state
return rvStageState
