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 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 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
with self.lock:
(self.stateVisual, isInArena) = self.TransformStateToFrame('Stage', state, doClipToArena=False)
self.posVisual = self.Get1FromPt(self.stateVisual.pose.position)
def HomeStage_callback(self, reqStageState):
while not self.bInitialized:
rospy.sleep(0.1)
with self.lock:
try:
self.Park_joint1()
except (rospy.ServiceException, rospy.exceptions.ROSInterruptException, IOError), e:
rospy.logwarn ('MA FAILED %s' % e)
rvStageState = SrvFrameStateResponse()
rospy.loginfo ('MA HomeStage_callback rvStageState=%s' % rvStageState)
return rvStageState
def Calibrate_callback(self, req):
# Integer values for directions - used in usb set/get
POSITIVE = 0
NEGATIVE = 1
# Convert parking coordinates to angles.
self.xPark = 0.0
self.yPark = 0.0
q1park = 0.0 #self.Get1FromXy(self.xPark, self.yPark)
q1origin = rospy.get_param('motorarm/angleOffset', 0.0) # Angle distance of index switch to 0 angle.
