def testCheckPickup(armName=Constants.Arm.Right): rospy.init_node('testCheckPickup',anonymous=True) imageDetector = ImageDetector() rospy.sleep(4) rospy.loginfo('Press enter to get {0} gripper point'.format(armName)) raw_input() gripperPose = imageDetector.getGripperPose(armName) convGripperPose = tfx.pose(Util.convertToFrame(gripperPose, Constants.Frames.Link0)) lastGripperPoint = convGripperPose.position.msg.PointStamped() lastGripperPoint.point.z += 0 rospy.loginfo('Press enter to thresh for red at last gripper point') raw_input() serviceName = '{0}_{1}'.format(Constants.Services.isFoamGrasped, armName) try: rospy.wait_for_service(serviceName, timeout=5) foamGraspedService = rospy.ServiceProxy(serviceName, ThreshRed) isFoamGrasped = foamGraspedService(lastGripperPoint).output if isFoamGrasped == 1: rospy.loginfo('Found red piece!') else: rospy.loginfo('Did not find red piece!') except e: rospy.loginfo('Thresh service failed: {0}'.format(e)) rospy.spin()
def __init__(self, armName): print('Initializing GripperFOVClass') self.armName = armName self.ravenArm = RavenArm(self.armName) self.imageDetector = ImageDetector() self.camFrame = '/left_BC' self.desiredOrientation = tfx.tb_angles(90,0,0) # in self.camFrame self.posFrame = MyConstants.Frames.Link0 self.rotFrame = MyConstants.Frames.RightTool if self.armName == MyConstants.Arm.Right else MyConstants.Frames.LeftTool self.pub = rospy.Publisher('orig_gripper_pose',PoseStamped) self.file = open('/tmp/GripperFOVTest.txt','w') print('Initialized GripperFOVClass')
class GripperFOVClass(): def __init__(self, armName): print('Initializing GripperFOVClass') self.armName = armName self.ravenArm = RavenArm(self.armName) self.imageDetector = ImageDetector() self.camFrame = '/left_BC' self.desiredOrientation = tfx.tb_angles(90,0,0) # in self.camFrame self.posFrame = MyConstants.Frames.Link0 self.rotFrame = MyConstants.Frames.RightTool if self.armName == MyConstants.Arm.Right else MyConstants.Frames.LeftTool self.pub = rospy.Publisher('orig_gripper_pose',PoseStamped) self.file = open('/tmp/GripperFOVTest.txt','w') print('Initialized GripperFOVClass') def currentPose(self): currTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), self.camFrame) currDetectedPose = Util.convertToFrame(self.imageDetector.getGripperPose(self.armName), self.camFrame) currPoseInCamera = tfx.pose(currTfPose.position, currDetectedPose.orientation) return currPoseInCamera def homePose(self): homePose = self.currentPose() homePose.orientation = self.desiredOrientation return homePose def findNewGripper(self, timeout=1): timeout = Util.Timeout(timeout) self.imageDetector.ignoreOldGripper(self.armName) timeout.start() while not timeout.hasTimedOut(): if self.imageDetector.hasFoundNewGripper(self.armName): return tfx.pose(self.imageDetector.getGripperPose(self.armName)) return None def rotateUntilUnseen(self, axis, rotateBy): firstPose = gripperPose = Util.convertToFrame(self.findNewGripper(), MyConstants.Frames.Link0) firstTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), MyConstants.Frames.Link0) self.pub.publish(firstPose.msg.PoseStamped()) if axis == 'pitch': tb_rotate = tfx.tb_angles(0,rotateBy,0) origAngle = tfx.tb_angles(firstPose.orientation).pitch_deg elif axis == 'roll': tb_rotate = tfx.tb_angles(0,0,rotateBy) origAngle = tfx.tb_angles(firstPose.orientation).roll_deg else: return 0.0 totalRotation = 0.0 rotatePose = tfx.pose([0,0,0], tb_rotate) lastOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation lastPose = None while gripperPose is not None: #gripperPose = Util.convertToFrame(gripperPose, self.rotFrame) #tfPose = tfx.pose([0,0,0],frame=self.rotFrame) #tapeRotation = gripperPose.orientation #tfRotation = tfPose.orientation #IPython.embed() #return #tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) #gripperPose = Util.convertToFrame(gripperPose, tfGripperPose.frame) #endPose = tfx.pose(tfGripperPose.position, rotatePose.orientation.matrix * gripperPose.orientation.matrix) #self.ravenArm.goToGripperPose(endPose, duration=1) #endPose = Util.endPose(gripperPose,tfx.pose([0,0,0],rotatePose.orientation, frame=self.camFrame),frame=MyConstants.Frames.Link0) #deltaPose = tfx.pose([0,0,0], endPose.orientation) #self.ravenArm.goToGripperPoseDelta(deltaPose, duration=1) tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) tfGripperPose = Util.convertToFrame(tfGripperPose, self.camFrame) endPose = Util.endPose(tfGripperPose, tfx.pose([0,0,0],tb_rotate,frame=self.camFrame)) endPose = Util.convertToFrame(endPose, MyConstants.Frames.Link0) endPose.position = firstTfPose.position self.ravenArm.goToGripperPose(endPose, duration=1) print('Rotating...') rospy.sleep(.25) #self.ravenArm.goToGripperPoseDelta(rotatePose, duration=1) totalRotation += rotateBy #print('Total rotation = {0}'.format(totalRotation)) currOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation if lastOrientation is not None: angleBetween = Util.angleBetweenQuaternions(lastOrientation.msg.Quaternion(), currOrientation.msg.Quaternion()) #print('angleBetween = {0}'.format(angleBetween)) if angleBetween < rotateBy*.5: self.file.write('Hit physical limit\n') print('Hit physical limit') break lastOrientation = currOrientation rospy.sleep(.5) lastPose = Util.convertToFrame(gripperPose, self.camFrame) gripperPose = self.findNewGripper() if lastPose is not None: firstPose = Util.convertToFrame(firstPose, self.camFrame) if axis == 'pitch': firstPitch = tfx.tb_angles(firstPose.orientation).pitch_deg lastPitch = tfx.tb_angles(lastPose.orientation).pitch_deg return copysign(fabs(lastPitch-firstPitch),rotateBy) elif axis == 'roll': firstRoll = tfx.tb_angles(firstPose.orientation).roll_deg lastRoll = tfx.tb_angles(lastPose.orientation).roll_deg return copysign(fabs(lastRoll-firstRoll),rotateBy) return totalRotation def runTest(self): self.ravenArm.start() rospy.sleep(2) currPoseInCamera = self.currentPose() homePose = self.homePose() deltaPose = Util.deltaPose(currPoseInCamera, homePose, self.posFrame, self.rotFrame) self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2) # self.ravenArm.goToGripperPoseDelta(tfx.pose([0,0,0],tfx.tb_angles(0,30,0)),duration=2) #rotatePose = tfx.pose([0,0,0],tfx.tb_angles(0,45,0)) #IPython.embed() #return # z must always stay the same z = currPoseInCamera.position.z xMove = .03 yMove = .01 gridMoves = [[-xMove, 0, 0], [0, yMove, 0], [xMove, 0, 0], [0, -yMove, 0]] rotationAxes = ['pitch', 'roll'] for gridMove in gridMoves: print('Go to next place on grid, press enter') raw_input() deltaPose = Util.deltaPose(self.currentPose(), self.homePose()+gridMove, self.posFrame, self.rotFrame) homePose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) self.ravenArm.goToGripperPose(homePose) self.file.write('Home pose {0}\n'.format(homePose)) print('Home pose {0}'.format(homePose)) for axis in rotationAxes: if rospy.is_shutdown(): return rotateBy = 5.0 self.file.write('Rotating in positive direction on {0} axis\n'.format(axis)) print('Rotate in positive direction on {0} axis'.format(axis)) #raw_input() maxRotation = self.rotateUntilUnseen(axis, rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('Rotating in negative direction on {0} axis\n'.format(axis)) print('Rotate in negative direction on {0} axis'.format(axis)) #raw_input() minRotation = self.rotateUntilUnseen(axis, -rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('{0} range of motion is ({1},{2})\n'.format(axis,minRotation,maxRotation)) print('{0} range of motion is ({1},{2})'.format(axis,minRotation,maxRotation)) self.file.close() self.ravenArm.stop()