def execAndRecordTraj(arm=Constants.Arm.Right):
    rospy.init_node('execAndRecordTraj',anonymous=True)
    rospy.sleep(1)
    ravenArm = RavenArm(arm)
    ravenPlanner = RavenPlanner(arm)
    record = Test(arm)
    rospy.sleep(2)
    
    ravenArm.start()
    
    rospy.loginfo('Classes are initialized, press enter')
    raw_input()
    

    startPose = tfx.pose(ravenArm.getGripperPose())    
    
    x = .01
    y = .06
    z = .05
    deltaPoses = []
    deltaPoses.append(tfx.pose([x,0, -z],frame=Constants.Frames.Link0))
    #deltaPoses.append(tfx.pose([0,-y, 0],frame=Constants.Frames.Link0))
    deltaPoses.append(tfx.pose([-x,0,z],frame=Constants.Frames.Link0))
    #deltaPoses.append(tfx.pose([0,y, 0],frame=Constants.Frames.Link0))

    #record.startRecording()
    while not rospy.is_shutdown():
        for deltaPose in deltaPoses:
            startPose = tfx.pose(ravenArm.getGripperPose())
            endPose = tfx.pose(Util.endPose(startPose, deltaPose))
            ravenArm.goToGripperPose(endPose)
            rospy.sleep(4)
    record.stopRecording()
            
    rospy.loginfo('Press enter to print and exit')
    raw_input()
    
    ravenArm.stop()
    
    record.printPoses()
    
    IPython.embed()
class MasterClass():
    """
    Contains the master pipeline for RavenDebridement in the run method

    See control_pipeline.jpeg for the pipeline overview
    """
    def __init__(self, armName, imageDetector):
        """
        armName must be from Constants.Arm
        imageDetector is instance of ImageDetectionClass
        """
        self.armName = armName
        
        if (armName == Constants.Arm.Left):
            self.gripperName = Constants.Arm.Left
            self.toolframe = Constants.Frames.LeftTool
            #self.calibrateGripperState = ImageDetectionClass.State.CalibrateLeft
        else:
            self.gripperName = Constants.Arm.Right
            self.toolframe = Constants.Frames.RightTool
            #self.calibrateGripperState = ImageDetectionClass.State.CalibrateRight

        self.listener = tf.TransformListener()

        # initialize the three main control mechanisms
        # image detection, gripper control, and arm control
        self.imageDetector = imageDetector
        self.ravenArm = RavenArm(self.armName)

        self.receptaclePose = None
        self.homePose = None
        self.objectPose = None
        self.gripperPose = None

        # translation frame
        self.transFrame = Constants.Frames.Link0
        # rotation frame
        self.rotFrame = self.toolframe

        # height offset for foam
        self.objectHeightOffset = .005

        # in cm/sec, I think
        self.openLoopSpeed = .04

        self.gripperOpenCloseDuration = 2.5

        # debugging outputs
        self.des_pose_pub = rospy.Publisher('desired_pose', PoseStamped)
        self.obj_pub = rospy.Publisher('object_pose', PoseStamped)

        self.timeout = Util.Timeout(30)
        self.findGripperTimeout = Util.Timeout(2)

        self.rotateBy = -30

    def findReceptacle(self, failMethod=None, successMethod=None):
        failMethod = failMethod or self.findReceptacle
        successMethod = successMethod or self.findHome

        rospy.loginfo('Searching for the receptacle')
        if not self.imageDetector.hasFoundReceptacle():
            rospy.loginfo('Did not find receptacle')
            return failMethod
        self.receptaclePose = self.imageDetector.getReceptaclePose()

        rospy.loginfo('Found receptacle')
        return successMethod

    def findHome(self, failMethod=None, successMethod=None):
        failMethod = failMethod or self.findHome
        successMethod = successMethod or self.moveToReceptacle

        rospy.loginfo('Searching for the home position')
        if not self.imageDetector.hasFoundHome():
            rospy.loginfo('Did not find home position')
            return failMethod
        self.homePose = self.imageDetector.getHomePose()

        rospy.loginfo('Found home position')
        return successMethod


    def findObject(self, failMethod=None, successMethod=None):
        failMethod = failMethod or self.findObject
        successMethod = successMethod or self.findGripper

        # reset rotateBy
        self.rotateBy = -30

        rospy.loginfo('Searching for object point')
        # find object point and pose
        if not self.imageDetector.hasFoundObject():
            rospy.loginfo('Did not find object')
            return failMethod
        # get object w.r.t. toolframe
        self.objectPose = self.imageDetector.getObjectPose(self.toolframe)
        self.objectPose.pose.position.z += self.objectHeightOffset
        self.publishObjectPose(self.objectPose)

        rospy.loginfo('Found object')
        return successMethod
        
    def findGripper(self, failMethod=None, successMethod=None):
        failMethod = failMethod or self.rotateGripper
        successMethod = successMethod or self.moveNearObject

        rospy.loginfo('Searching for ' + self.gripperName)
        # find gripper point
        self.imageDetector.ignoreOldGripper(self.gripperName)

        self.findGripperTimeout.start()
        while (not self.imageDetector.hasFoundGripper(self.gripperName)) or (not self.imageDetector.hasFoundNewGripper(self.gripperName)):
            if self.findGripperTimeout.hasTimedOut():
                rospy.loginfo('Did not find gripper')
                return failMethod
            rospy.sleep(.05)

        self.gripperPose = self.imageDetector.getGripperPose(self.gripperName)

        rospy.loginfo('Found gripper')
        return successMethod

    def rotateGripper(self, failMethod=None, successMethod=None):
        failMethod = failMethod or self.findGripper
        successMethod = successMethod or self.findGripper
        
        global numGripperRotations
        numGripperRotations += 1
        
        rospy.loginfo('Rotating the gripper by ' + str(self.rotateBy) + ' degrees')
        deltaPose = tfx.pose([0,0,.001], tfx.tb_angles(0,0,self.rotateBy))
        self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2)

        self.rotateBy = -math.copysign(abs(self.rotateBy)+5, self.rotateBy)

        return successMethod
    
    def moveNearObject(self, failMethod=None, successMethod=None):
        """
        Goes near the object

        Returns servoToObject
        """
        failMethod = failMethod or self.moveNearObject
        successMethod = successMethod or self.servoToObject
        
        rospy.loginfo('Moving near the object point')
        # go to near the object point
        
        deltaPose = Util.deltaPose(self.gripperPose, self.objectPose, self.transFrame, self.rotFrame)
        deltaPose.position.z += .03
        
        self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed)

        return successMethod

    def servoToObject(self, failMethod=None, successMethod=None):
        """
        In close-loop, servos to the object. Then, closes the gripper.
        """
        failMethod2 = lambda: self.findGripper(self.moveToHome, self.servoToObject)
        failMethod1 = lambda: self.findObject(self.moveToHome, failMethod2)
        failMethod0 = lambda vertAmount: self.moveVertical(self.moveToReceptacle, failMethod1, vertAmount)
        failMethod = failMethod or failMethod0
        successMethod = successMethod or self.moveVertical

        rospy.loginfo('Servoing to the object point')
        # servo to the object point
            
        transBound = .008
        rotBound = float("inf")

        maxMovement = .015
        deltaPose = uncappedDeltaPose = tfx.pose([0,0,0])

        # if can't find gripper at start, go back to receptacle
        rospy.sleep(1)
        if self.imageDetector.hasFoundGripper(self.gripperName) and self.imageDetector.hasFoundNewGripper(self.gripperName):
            self.gripperPose = self.imageDetector.getGripperPose(self.gripperName)
        else:
            return self.moveToHome
            
        self.timeout.start()
        while not Util.withinBounds(self.gripperPose, self.objectPose, transBound, rotBound, self.transFrame, self.rotFrame):
                
            if self.ravenArm.isPaused():
                rospy.sleep(1)
                if self.imageDetector.hasFoundNewGripper(self.gripperName):
                    rospy.loginfo('paused and found new gripper')
                    self.gripperPose = self.imageDetector.getGripperPose(self.gripperName)
                    deltaPose = uncappedDeltaPose = tfx.pose(Util.deltaPose(self.gripperPose, self.objectPose, Constants.Frames.Link0, self.toolframe))
                    
                    deltaPose.position = deltaPose.position.capped(maxMovement)
                    #if abs(deltaPose.position.z) > maxMovement:
                    #    deltaPose.position.z = math.copysign(maxMovement, deltaPose.position.z)

                    deltaPose0Link = tfx.pose(Util.deltaPose(self.gripperPose, self.objectPose, Constants.Frames.Link0, Constants.Frames.Link0))
                    deltaPose0Link.position = deltaPose.position.capped(maxMovement)
                    self.publishDesiredPose(deltaPose0Link, tfx.pose(self.gripperPose))

                    rospy.loginfo('delta pose')
                    rospy.loginfo(deltaPose)

                    self.ravenArm.goToGripperPoseDelta(deltaPose)
                    rospy.sleep(1)
                else:
                    rospy.loginfo('paused but did NOT find new gripper')
                    deltaPose.position = uncappedDeltaPose.position - deltaPose.position
                    self.ravenArm.goToGripperPoseDelta(deltaPose, ignoreOrientation=True)
                    break
                    
                    
            if self.timeout.hasTimedOut() or rospy.is_shutdown():
                rospy.loginfo('Timed out')
                self.objectHeightOffset = self.objectHeightOffset - .0005 if self.objectHeightOffset > 0 else 0
                global numFailedPickups
                numFailedPickups += 1
                return failMethod(.02)
                
            rospy.sleep(.1)

        rospy.loginfo('Closing the gripper')
        # close gripper (consider not all the way)
        self.ravenArm.closeGripper(duration=self.gripperOpenCloseDuration)
            
        return successMethod

    def moveVertical(self, failMethod=None, successMethod=None, vertAmount=.05):
        """
        Move vertical in open-loop
        """
        failMethod = failMethod or self.checkPickup
        successMethod = successMethod or self.checkPickup

        rospy.loginfo('Moving vertical with the object')
        # move vertical with the object
        deltaPose = tfx.pose([0,0,vertAmount]).msg.Pose()
        
        self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed)
        
        return successMethod
    
    def checkPickup(self, failMethod=None, successMethod=None):
        """
        Checks if the grasper picked up a red foam piece
        """
        failMethod = failMethod or (lambda: self.findObject(self.moveToHome, (lambda: self.findGripper(self.moveToHome, self.servoToObject))))
        successMethod = successMethod or (lambda: self.moveToHome(self.moveToReceptacle, self.moveToReceptacle))

        rospy.loginfo('Check if red foam piece successfully picked up')
        rospy.sleep(1)

        try:
            rospy.wait_for_service(Constants.Services.isFoamGrasped, timeout=5)
            foamGraspedService = rospy.ServiceProxy(Constants.Services.isFoamGrasped, ThreshRed)
            isFoamGrasped = foamGraspedService(0).output
            
            if isFoamGrasped == 1:
                rospy.loginfo('Successful pickup!')
                return successMethod
            else:
                rospy.loginfo('Failed pickup')
                
                global numFailedPickups
                numFailedPickups += 1
                
                rospy.loginfo('Opening the gripper')
                # open gripper (consider not all the way)
                self.ravenArm.openGripper(duration=self.gripperOpenCloseDuration)
                
                return failMethod
        except:
            rospy.loginfo('Service exception, assuming successful pickup')
            return successMethod
            
            
    
    def moveToReceptacle(self, failMethod=None, successMethod=None):
        """
        Move to the receptacle in open-loop
        Then, open the gripper
        """
        failMethod = failMethod or self.moveToHome
        successMethod = successMethod or self.moveToHome

        rospy.loginfo('Moving to receptacle')
        # move to receptacle with object
                
        self.ravenArm.goToGripperPose(self.receptaclePose.pose, ignoreOrientation=True, speed=self.openLoopSpeed)


        rospy.loginfo('Opening the gripper')
        # open gripper (consider not all the way)
        self.ravenArm.openGripper(duration=self.gripperOpenCloseDuration)
                
        return successMethod

    def moveToHome(self, failMethod=None, successMethod=None):
        """
        Move to the home position in open-loop
        """
        failMethod = failMethod or self.findObject
        successMethod = successMethod or self.findObject

        rospy.loginfo('Moving to home position')
        # move to home position
        
        self.ravenArm.goToGripperPose(self.homePose.pose, ignoreOrientation=True, speed=self.openLoopSpeed)

        # so when finding object, find newest one
        self.imageDetector.removeObjectPoint()
        
        return successMethod
    
    def run(self):
        """
        Loops through the pipeline

        The pipeline consists of the above methods.
        Each method has a fail and success return method.
        """
        delay = .5
        currentStage = self.findReceptacle
        
        self.ravenArm.start()
        
        if WRITE_TO_FILE:
            import datetime
            startTime = rospy.Time.now()
            fileName = '/home/gkahn/ros_workspace/RavenDebridement/test_files/OldMasterTest_{0}'.format(datetime.datetime.now())
            file = open(fileName,'w')

        while not rospy.is_shutdown():
            rospy.loginfo('Next stage')
            #rospy.loginfo('Press enter')
            #raw_input()
            if WRITE_TO_FILE:
                stageStartTime = rospy.Time.now()
                file.write('{0}, Start time = {1}\n'.format(currentStage.__name__,stageStartTime.to_sec()-startTime.to_sec()))
                
            currentStage = currentStage()
            
            if WRITE_TO_FILE:
                stageEndTime = rospy.Time.now()
                file.write('End time = {0}, Stage duration = {1}\n\n'.format(stageEndTime.to_sec()-startTime.to_sec(),stageEndTime.to_sec()-stageStartTime.to_sec()))
            rospy.sleep(delay)
            
        if WRITE_TO_FILE:
            file.write('Total elapsed time = {0}\n'.format(rospy.Time.now().to_sec()-startTime.to_sec()))
            file.write('Number of failed pickups = {0}\n'.format(numFailedPickups))
            file.write('Number of gripper rotations = {0}\n'.format(numGripperRotations))
            file.close()

        self.ravenArm.stop()

    def publishObjectPose(self, pose):
        #self.obj_pub.publish(pose)
        pass

    def publishDesiredPose(self, delta_pose, gripper_pose):
        """
        frame = gripper_pose.frame
        time = self.listener.getLatestCommonTime(frame, Constants.Frames.Link0)
        pose_stamped = gripper_pose.msg.PoseStamped()
        pose_stamped.header.stamp = time
        gripper_pose = self.listener.transformPose(Constants.Frames.Link0, gripper_pose.msg.PoseStamped())

        pose = PoseStamped()
        pose.header.frame_id = Constants.Frames.Link0
        pose.header.stamp = rospy.Time.now()
        pose.pose.position = (gripper_pose.pose.position + delta_pose.position).msg.Point()

        ori = tfx.pose(gripper_pose).orientation.matrix * delta_pose.orientation.matrix
        pose.pose.orientation = tfx.pose([0,0,0], ori).orientation.msg.Quaternion()
        self.des_pose_pub.publish(pose)
        """
        pass
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()