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