def listener():
    
           
    
    tf_listener = tf.TransformListener()
    global MSG, MSGR1, MSGR2, i
    while not rospy.is_shutdown():
	try:
		now = rospy.Time.now()
        	(trans,rot) = tf_listener.lookupTransform("/pioneer1/odom", "/map", rospy.Time(0))
		(roll,pitch,yaw) = euler_from_quaternion(rot)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
		print "tf error"
	try:
		now = rospy.Time.now()
        	(trans,rot) = tf_listener.lookupTransform("/pioneer2/odom", "/map", rospy.Time(0))
		(roll,pitch,yaw) = euler_from_quaternion(rot)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
		print "tf error"
        
	#MSGR1=np.resize(MSGR1,[i+1,len(MSG[0][:])])
        MSGR1[i][:]=np.array(MSG[0][:])
	#MSGR2=np.resize(MSGR2,[i+1,len(MSG[1][:])])
	print i
        MSGR2[i][:]=np.array(MSG[1][:])
	#obj_arr = np.zeros((4,), dtype=np.object)
        #obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
	#obj_arr[1] = MSGR1
	#obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
	#obj_arr[3] = MSGR2
        #scipy.io.savemat('np_cells.mat', {'obj_arr':obj_arr})

	#f.write('{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang1))
	#rob=1	
	#f.write(',{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang2))
	#f.write('\n')
	#MSG[0][:]=np.zeros(18)
	#MSG[1][:]=np.zeros(18)
        i=i+1
	if i==6000:
		obj_arr = np.zeros((4,), dtype=np.object)
        	obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
		#print MSGR1
		obj_arr[1] = MSGR1
		obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
		obj_arr[3] = MSGR2
        	scipy.io.savemat('np_cells.mat', {'obj_arr':obj_arr})
		 
	rate.sleep()

 def hover(self):
     with robotLock:
         self.R.set_speed(speed=[10, 10, 50, 50])
         with print_lock:
             print "Speeding up to 10% 'hover'"
         self.printDateTime()
         print >> self.f, "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2],
         self.f.flush()
         while (self.R.get_cartesian() != self.HoverPosition):
             self.R.set_cartesian(self.HoverPosition)
             time.sleep(1)
         self.R.set_speed(speed=[0.5, 0.5, 50, 50])
         with print_lock:
             print "Slowing Down to 1% 'hover'"
         cartesian    = self.R.get_cartesian()
         euler        = self.r2d(list(t.euler_from_quaternion(cartesian[1])))
         euler        = [round(euler[i], 1) for i in range(len(euler))]
         cartesian[1] = euler
         self.printDateTime()
         print >> self.f, "Hovering at: ", cartesian
         self.f.flush()
         time.sleep(0.2)
         saveImgEvent.clear()
         if not saveImgEvent.wait(1):
             print "Camera not working"

def Hover(HoverPosition):
    global R, f
    with robotLock:
        R.set_speed(speed=[10, 10, 50, 50])
        with print_lock:
            print "Speeding up to 10% 'hover'"
            printDateTime()
            print >> f, "Heading to Z coordinate %.1fmm" % HoverPosition[0][2],
            f.flush()
        while (R.get_cartesian() != HoverPosition):
            R.set_cartesian(HoverPosition)
            time.sleep(1)
            with print_lock:
                a = R.get_cartesian()
                print a, HoverPosition
        R.set_speed(speed=[1, 1, 50, 50])
        with print_lock:
            print "Slowing Down to 1% 'hover'"
        cartesian    = R.get_cartesian()
        euler        = r2d(list(t.euler_from_quaternion(cartesian[1])))
        euler        = [round(euler[i], 1) for i in range(len(euler))]
        cartesian[1] = euler
        with print_lock:
            printDateTime()
            print >> f, "Hovering at: ", cartesian,
            f.flush()
        time.sleep(0.2)
        saveImgEvent.clear()
        saveImgEvent.wait(1)

def Home(HomePos):
    global R, f
    with robotLock:
        R.set_speed(speed=[10, 10, 50, 50])
        with print_lock:
            print "Speeding up to 10%"
            printDateTime()
            print >> f, "Heading to Home Position",
            f.flush()
        while (R.get_joints() != HomePos):
            R.set_joints(HomePos)
            time.sleep(0.5)
            with print_lock:
                a = R.get_joints()
                print a, HomePos
        R.set_speed(speed=[1, 1, 50, 50])
        with print_lock:
            print "Slowing Down to 1%"
        cartesian    = R.get_cartesian()
        euler        = r2d(list(t.euler_from_quaternion(cartesian[1])))
        euler        = [round(euler[i], 1) for i in range(len(euler))]
        cartesian[1] = euler
        with print_lock:
            printDateTime()
            print >> f, "Arrived at Home Position: ", cartesian
            f.flush()

    def calc_vel(self,pos_x,pos_y,quater):
        global odom_key
        if odom_key==True:
            self.first_x=pos_x
            self.first_y=pos_y
            self.x_goal+=self.first_x
            self.y_goal+=self.first_y
            odom_key=False
        
        rel_x=self.x_goal-pos_x
        rel_y=self.y_goal-pos_y
        dest_angle=math.atan2(rel_y,rel_x)
        quaterArr=np.array([quater.x,quater.y,quater.z,quater.w])
        robot_euler=transformations.euler_from_quaternion(quaterArr,'sxyz')
        robot_angle=robot_euler[2]
        angle=dest_angle-robot_angle
        if math.sqrt(rel_x**2+rel_y**2) < 0.1:
            self.v=0.0;
        else :
            self.v=self.odom_init_v


        if angle > 0.3 :
            self.w=self.odom_init_w
        elif angle < -0.3:
            self.w=-self.odom_init_w
        else :
            self.w=0.0

        return (self.v,self.w)

 def hover(self):
     with robotLock:
         self.R.set_speed(speed=[10, 10, 50, 50])
         print "Speeding up to 10% 'hover'"
         self.printDateTime()
         print >> self.f, "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2],
         print "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2]
         self.f.flush()
         count = 0
         while (self.R.get_cartesian() != self.HoverPosition):
             self.R.set_cartesian(self.HoverPosition)
             time.sleep(1)
             a = self.R.get_cartesian()
             print a, self.HoverPosition
             if count >= 3:
                 self.R.set_speed(speed=[0.5, 0.5, 50, 50])
             count += 1
         self.R.set_speed(speed=[0.5, 0.5, 50, 50])
         print "Slowing Down to 1% 'hover'"
         cartesian    = self.R.get_cartesian()
         euler        = self.r2d(t.euler_from_quaternion(cartesian[1]))
         euler        = [round(euler[i], 1) for i in range(len(euler))]
         cartesian[1] = euler
         self.printDateTime()
         print >> self.f, "Hovering at: ", cartesian
         self.f.flush()
         time.sleep(0.2)
         saveImage()

 def getWeight(self):
     self.LoadCellArray = [("Time", "ADC", "Weight", "avgWeight", "cartesian[0][0]", "cartesian[0][1]", "cartesian[0][2]", "cartesian[1][0]", "cartesian[1][1]", "cartesian[1][2]", "cartesian[1][3]")]
     writeList2File(os.path.join("TSP_Pictures", "%dgrams" % self.kg, "%02d" % self.numFolders, "LoadCellData.txt"), self.LoadCellArray)
     while checkData.wait(0) and not end.wait(0):
         try:
             scaleArray = q1.get(True, 0.1)
             q1.task_done()
         except Queue.Empty:
             break
         if robotLock.acquire(False):
             cartesian   = self.R.get_cartesian()
             cartesian1  = copy.copy(cartesian)
             robotLock.release()
         else:
             cartesian   = copy.copy(cartesian1)
         touchDownQueue.put(cartesian)
         cartesian[1]       = self.r2d(list(t.euler_from_quaternion(cartesian[1], 'sxyz')))
         checktime          = time.time()
         self.LoadCellArray = ((round((checktime - self.startclock), 6), scaleArray[0], scaleArray[1], scaleArray[2], cartesian1[0][0], cartesian1[0][1], cartesian1[0][2], cartesian1[1][0], cartesian1[1][1], cartesian1[1][2], cartesian1[1][3]))
         writeList2File(os.path.join("TSP_Pictures", "%dgramsSH" % self.kg, "%02d" % self.numFolders, "LoadCellData.txt"), self.LoadCellArray)
         Z1.put(scaleArray)
         Z2.put(scaleArray[2])
         Z.set()
         time.sleep(0.2)
     with print_lock:
         print "No Data1"

 def _process_root_orientation(self):
     vertical_orientation = euler_from_quaternion(
         self.get_joint("torso").get_orientation(), axes="rxyz")[1]
     clamped_vertical_orientation = self._root_orientation_clamper.clamp(
         vertical_orientation)
     self._root_orientation_smoother.update(clamped_vertical_orientation)
     self._smoothed_root_vertical_orientation = self._root_orientation_smoother.get_value()

def imu_callback(imu):
    global imu_euler, imu_quaternion
    imu_quaternion = (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w)
    roll, pitch, yaw = euler_from_quaternion(imu_quaternion, axes="sxyz")
    imu_euler = RPY()
    imu_euler.roll = roll
    imu_euler.pitch = pitch
    imu_euler.yaw = yaw

def new_marker(M, id, x, y, z, q1, q2, q3, q4):
    q4_ = math.sqrt(1.0 - q1 * q1 - q2 * q2 - q3 * q3)
    e = transformations.euler_from_quaternion([q1, q2, q3, q4_])

    if str(id) not in M:
        M[str(id)] = [[x, y, z, e[0], e[1], e[2]]]
    else:
        # print M[str(id)]
        M[str(id)].append([x, y, z, e[0], e[1], e[2]])

def lasers_callback(data):
    global report
    quaternion = (data.pose.orientation.x, data.pose.orientation.y, \
                  data.pose.orientation.z, data.pose.orientation.w)
    roll, pitch, yaw = euler_from_quaternion(quaternion, axes="sxyz")
    report.lasers_pose.position = data.pose.position
    report.lasers_pose.orientation.roll = roll
    report.lasers_pose.orientation.pitch = pitch
    report.lasers_pose.orientation.yaw = yaw

 def _process_vertical_axis(self, euler_angles, axes):
     if axes[1] == self._vertical_axis:
         return self._process_vertical_orientation_as_first_axis(euler_angles)
     else:
         if self._vertical_axis == "y":
             axes_with_vertical_first = "ryxz"
         elif self._vertical_axis == "z":
             axes_with_vertical_first = "rzxy"
         euler_angles_vertical_axis_first = euler_from_quaternion(
             quaternion_from_euler(*euler_angles, axes=axes),
             axes=axes_with_vertical_first)
         euler_angles_vertical_axis_first_reoriented = \
             self._process_vertical_orientation_as_first_axis(
             euler_angles_vertical_axis_first)
         return euler_from_quaternion(
             quaternion_from_euler(
                 *euler_angles_vertical_axis_first_reoriented,
                  axes=axes_with_vertical_first),
             axes=axes)

    def saveModelStates (self, model_states):
        # Model states comes in as a list of all entities in the enviroment
        m = model_states.name.index ("mobile_base")

        self.turtlebot_position = model_states.pose[m].position
        self.turtlebot_orientation = model_states.pose[m].orientation

        # Getting yaw (in x, y, z) rather than a quaternion
        self.euler_angles = tf.euler_from_quaternion([self.turtlebot_orientation.w, self.turtlebot_orientation.x, self.turtlebot_orientation.y, self.turtlebot_orientation.z])

        # Yaw is in range [0, 360)
        self.yaw = degrees ((pi - (self.euler_angles[0])))
        self.compass = (self.yaw - degrees (pi/4)) // degrees ((pi / 2))
 
        if self.compass == -1.0:
            self.compass = 3.0

    def getPos(self):
        rate=rospy.Rate(10.0)

        while not rospy.is_shutdown():
            print "here1"
            try:

                #Pulled from here: http://answers.ros.org/question/10268/where-am-i-in-the-map/
#                (trans,rot) = self.listener.lookupTransform('map', 'odom', rospy.Time(0))
                (trans,rot) = self.listener.lookupTransform('map', 'base_link', rospy.Time(0))
                print "after lookup"
                rot=transformations.euler_from_quaternion(rot)
                return (trans,rot)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException ) as e:
                print e
                continue
            rate.sleep()

def callback2(odom,rob):
    global MSG
    #MSG[rob][:]=numpy.zeros(18)
    MSG[rob][0]=odom.pose.pose.position.x
    MSG[rob][1]=odom.pose.pose.position.y
    MSG[rob][2]=odom.pose.pose.position.z
    rot = np.empty((4, ), dtype=np.float64)
    rot[0]=odom.pose.pose.orientation.x
    rot[1]=odom.pose.pose.orientation.y
    rot[2]=odom.pose.pose.orientation.z
    rot[3]=odom.pose.pose.orientation.w
    (roll,pitch,yaw) = euler_from_quaternion(rot)
    MSG[rob][3]= round(yaw,5)
    MSG[rob][4]= odom.twist.twist.linear.x
    MSG[rob][5]= round(odom.twist.twist.angular.z,4)
    MSG[rob][6]= odom.header.stamp.secs
    MSG[rob][7]= odom.header.stamp.nsecs

 def record(self):
     self.Array = [("Time", "cartesian[0][0]", "cartesian[0][1]", "cartesian[0][2]", "cartesian[1][0]", "cartesian[1][1]", "cartesian[1][2]", "cartesian[1][3]")]
     writeList2File(os.path.join(self.DIR, "RunData.txt"), self.Array)
     while not end.wait(0):
         if robotLock.acquire(False):
             cartesian  = self.R.get_cartesian()
             cartesian1 = copy.deepcopy(cartesian)
             robotLock.release()
         else:
             cartesian = copy.deepcopy(cartesian1)
         touchDownQueue.put(cartesian)
         cartesian[1] = self.r2d(t.euler_from_quaternion(cartesian[1], 'sxyz'))
         checktime    = time.time()
         self.Array   = ((round((checktime - self.startclock), 6), cartesian1[0][0], cartesian1[0][1], cartesian1[0][2], cartesian1[1][0], cartesian1[1][1], cartesian1[1][2], cartesian1[1][3]))
         writeList2File(os.path.join(self.DIR, "RunData.txt"), self.Array)
         ok.set()
         time.sleep(0.2)
     with print_lock:
         print "No Data1"

def computeCameraPoseFromAircraft(image, cam, ref,
                                  yaw_bias=0.0, roll_bias=0.0,
                                  pitch_bias=0.0, alt_bias=0.0):
    lla, ypr, ned2body = image.get_aircraft_pose()
    aircraft_lat, aircraft_lon, aircraft_alt = lla
    #print "aircraft quat =", world2body
    msl = aircraft_alt + image.alt_bias + alt_bias

    (camera_yaw, camera_pitch, camera_roll) = cam.get_mount_params()
    body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
                                                     camera_pitch * d2r,
                                                     camera_roll * d2r,
                                                     'rzyx')
    ned2cam = transformations.quaternion_multiply(ned2body, body2cam)
    (yaw, pitch, roll) = transformations.euler_from_quaternion(ned2cam, 'rzyx')
    ned = navpy.lla2ned( aircraft_lat, aircraft_lon, aircraft_alt,
                         ref[0], ref[1], ref[2] )
    #print "aircraft=%s ref=%s ned=%s" % (image.get_aircraft_pose(), ref, ned)
    return (ned.tolist(), [yaw/d2r, pitch/d2r, roll/d2r])

 def Home(self):
     with robotLock:
         self.R.set_speed(speed=[10, 10, 50, 50])
         print "Speeding up to 10%"
         self.printDateTime()
         print >> self.f, "Heading to Home Position",
         self.f.flush()
         while (self.R.get_joints() != self.homePos):
             self.R.set_joints(self.homePos)
             time.sleep(4)
         self.R.set_speed(speed=[0.5, 0.5, 50, 50])
         print "Slowing Down to 1%"
         cartesian    = self.R.get_cartesian()
         euler        = self.r2d(t.euler_from_quaternion(cartesian[1]))
         euler        = [round(euler[i], 1) for i in range(len(euler))]
         cartesian[1] = euler
         self.printDateTime()
         print >> self.f, "Arrived at Home Position: ", cartesian
         self.f.flush()

def quat2ypr(quaternions,method='kevin'):
    """
        Purpose: Convert a list of arrays of quaternions to Euler angles (roll,pitch,yaw)
        Inputs: quaternions - list of quaternion arrays. In the form of 'SXYZ' I THINK?!?!?
        Outputs: r,p,y - separate roll, pitch, and yaw vectors
        Can test with:
            >>>quats=tfdat /= abs(fdat).max()racking.sample_quats()
            >>>[r,p,y]=tracking.quat2rpy(quats)
    """
    roll=[]
    pitch=[]
    yaw=[]
    for q in quaternions:
        if method is 'transform':
            YPR=transform.euler_from_quaternion(q, axes='rzyx')   # default is 'sxyz'
        else:
            YPR= quat2euler321(q)
        yaw.append(YPR[0])
        pitch.append(YPR[1])
        roll.append(YPR[2])
    return yaw,pitch,roll

 def hover(self):
     with robotLock:
         initErrorBds.set()
         self.error = 5
         self.R.set_speed(speed=[10, 10, 50, 50])
         with print_lock:
             print "Speeding up to 10% 'hover'"
         self.printDateTime()
         print >> self.f, "Heading to Z coordinate 345mm",
         self.f.flush()
         while (self.R.get_cartesian() != self.HoverPosition):
             self.R.set_cartesian(self.HoverPosition)
             time.sleep(1)
         self.R.set_speed(speed=[0.5, 0.5, 50, 50])
         with print_lock:
             print "Slowing Down to 1% 'hover'"
         cartesian    = self.R.get_cartesian()
         euler        = self.r2d(list(t.euler_from_quaternion(cartesian[1])))
         euler        = [round(euler[i], 1) for i in range(len(euler))]
         cartesian[1] = euler
         self.printDateTime()
         print >> self.f, "Hovering at: ", cartesian
         self.f.flush()
         return cartesian