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.