def talker():
    UDP_IP = "192.168.0.107"
    UDP_PORT = 9002

    sock = socket.socket(socket.AF_INET, # Internet
                         socket.SOCK_DGRAM) # UDP
    sock.bind((UDP_IP, UDP_PORT))
    pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
    rospy.init_node('test_relay', anonymous=True)
    rate = rospy.Rate(100) # 10hz
    while not rospy.is_shutdown():
       data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
       print "received message:", data
       data = re.sub("[A-Za-z]","",str(data))
       result = str(data).split(":")
       print result
       rospy.loginfo(result)
       
       
       msg = NavSatFix()
       msg.header = Header()
       msg.latitude = float(result[1])
       msg.longitude = float(result[2])
       #rospy.loginfo(msg)
       pub.publish(msg)

def got_position(xx, yy, aa, stamp):
    global EP

    fix = NavSatFix()
    fix.header.stamp = stamp
    fix.header.frame_id = "sim_gps"
    fix.status.status = 0
    fix.status.service = 1

    utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP)
    utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP)
    (lon, lat) = conv(utm_e, utm_n, inverse=True)

    fix.latitude = lat
    fix.longitude = lon
    fix.altitude = 1.3

    # print "got_position: %f %f" % (lat, lon)

    gps.publish(fix)
    pos.publish(Pose2D(utm_e, utm_n, aa))
    tfBr.sendTransform(
        (utm_e - base_xx, utm_n - base_yy, 0),
        tf.transformations.quaternion_from_euler(0, 0, 0),
        rospy.Time.now(),
        "/map",
        "/gps",
    )

def joy_callback(msg):
    if msg.buttons[12] == 1:
        gps_goal = NavSatFix()
        gps_goal.latitude = 33.726526
        gps_goal.longitude = -83.299984
        gps_put.publish(gps_goal)
    if msg.buttons[14] == 1:
        client.cancel_all_goals()

 def publish_gps(self):
     if self.drone.positionGPS!=None:
        gps_data = NavSatFix()
        gps_data.latitude = self.drone.positionGPS[0]
        gps_data.longitude = self.drone.positionGPS[1]#maybe reversed?
        gps_data.altitude = self.drone.positionGPS[2]
        gps_data.header.frame_id = self.unique_id
        self.gps_pub.publish(gps_data)

def getMsg():
	
	pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix)
	pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped)
	
	#Initialize the RTKLIB ROS node	
	rospy.init_node('rtklib_messages', anonymous=True)
	
	#Define the publishing frequency of the node
	rate = rospy.Rate(10)
	
	#Create a socket
	sock = socket.socket()
	
	#Get the address of the local host
	host = socket.gethostname()
	
	#Connect to the RTKRCV server that is bound to port xxxx
	port = 5801
	sock.connect((host,port))
	
	e2 = 6.69437999014e-3
	a = 6378137.0
	
	while not rospy.is_shutdown():
		
		navsat = NavSatFix()
		ecef_xyz = Point()
		ecef_pose = Pose()
		ecef_stampedPose = PoseStamped()
		
		ecef_stampedPose = 
		navsat.header.stamp = rospy.Time.now()
		
		#Get the position message from the RTKRCV server
		msgStr = sock.recv(1024)
		
		#Split the message
		msg = msgStr.split()
		
		navsat.latitude = float(msg[2])
		navsat.longitude = float(msg[3])
		navsat.altitude = float(msg[4])
		
		N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
		
		ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
		ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
		ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
		
		ecef_pose.position = ecef_xyz
		ecef_stampedPose.pose = ecef_pose
		
		pub_navsat.publish(navsat)
		pub_pose.publish(ecef_stampedPose)
		
		rate.sleep()

    def default(self, ci='unused'):
        gps = NavSatFix()
        gps.header = self.get_ros_header()

        gps.latitude = self.data['x']
        gps.longitude = self.data['y']
        gps.altitude = self.data['z']

        self.publish(gps)

	def recieve_gps(self, data ):
		msg = NavSatFix()
		msg.latitude = data.vector.x 
		msg.longitude = data.vector.y 
		msg.altitude = data.vector.z

		msg.header.stamp = data.header.stamp
		msg.header.frame_id = data.header.frame_id

		self.publisher['gps'].publish(msg)

    def default(self, ci='unused'):
        """ Publish the data of the gps sensor as a custom ROS NavSatFix message """
        gps = NavSatFix()
        gps.header = self.get_ros_header()

        # TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html
        gps.latitude = self.data['x']
        gps.longitude = self.data['y']
        gps.altitude = self.data['z']

        self.publish(pose)

def address2geo(address):
    gurl = 'https://maps.googleapis.com/maps/api/geocode/xml?address='+address
    res = urllib.urlopen(gurl).read()
    ans = NavSatFix()
    i1 = res.find('lat')
    i2 = res.find('/lat',i1)
    ans.latitude = float(res[i1+4:i2-1])
    i1 = res.find('lng')
    i2 = res.find('/lng',i1)
    ans.longitude = float(res[i1+4:i2-1])
    return ans

def gps_fix_sim():
    pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10)
    rospy.init_node('gps_sim', anonymous=True)
    rate = rospy.Rate(2)
    
    while not rospy.is_shutdown():
       gps_fix = NavSatFix()
       gps_fix.latitude = 36.6
       gps_fix.longitude = -121.1
       gps_fix.altitude = 1.0
       pub.publish(gps_fix)
       rate.sleep()

def pub_gps(msg_type, msg, bridge):
    pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
    fix = NavSatFix()
    fix.header.stamp = bridge.project_ap_time(msg)
    fix.header.frame_id = 'base_footprint'
    fix.latitude = msg.lat/1e07
    fix.longitude = msg.lon/1e07
    # NOTE: absolute (MSL) altitude
    fix.altitude = msg.alt/1e03
    fix.status.status = NavSatStatus.STATUS_FIX
    fix.status.service = NavSatStatus.SERVICE_GPS
    fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
    pub.publish(fix)

 def convert_gps_to_pose(self, coords):
     # create NavSatFix msg
     nav_msg = NavSatFix()
     nav_msg.header.stamp = rospy.get_rostime()
     nav_msg.header.frame_id = 'odom'
     # x is longitude, y is latitude
     nav_msg.longitude = coords['x']
     nav_msg.latitude = coords['y']
     # convert using /latlong_goal_node/AddLatLongGoal service
     # returns PoseStamped
     response = self.latlong_service(nav_msg)
     rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x,
                   response.goal_xy.pose.position.y))
     return response.goal_xy

    def bestpos_handler(self, bestpos):
        navsat = NavSatFix()

        # TODO: The timestamp here should come from SPAN, not the ROS system time.
        navsat.header.stamp = rospy.Time.now()
        navsat.header.frame_id = self.odom_frame

        # Assume GPS - this isn't exposed
        navsat.status.service = NavSatStatus.SERVICE_GPS

        position_type_to_status = {
            BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
            BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
        }
        navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX)

        # Position in degrees.
        navsat.latitude = bestpos.latitude
        navsat.longitude = bestpos.longitude

        # Altitude in metres.
        navsat.altitude = bestpos.altitude
        navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
        navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
        navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

        # Ship ito
        self.pub_navsatfix.publish(navsat)

 def testAutoOriginFromNavSatFix(self):
     rospy.init_node('test_initialize_origin')
     nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2)
     origin_sub = self.subscribeToOrigin()
     self.test_stamp = True
     nsf_msg = NavSatFix()
     nsf_msg.latitude = swri['latitude']
     nsf_msg.longitude = swri['longitude']
     nsf_msg.altitude = swri['altitude']
     nsf_msg.header.frame_id = "/far_field"
     nsf_msg.header.stamp = msg_stamp
     r = rospy.Rate(10.0)
     while not rospy.is_shutdown():
         nsf_pub.publish(nsf_msg)
         r.sleep()

def calc_lat_long(init_pos,distance,brng):
  '''
    * calculate the latitude and longitude given distance and bearing
    * distnace input in [m] brng in [rad]
  '''
  R = 6371.0 * 1000
  p2 = NavSatFix()
  lat1 = init_pos.latitude*PI/180.0
  lon1 = init_pos.longitude*PI/180.0
	  
  p2.latitude = (asin( sin(lat1)*cos(distance/R) + cos(lat1)*sin(distance/R)*cos(brng)))
  p2.longitude = (lon1 + atan2(sin(brng)*sin(distance/R)*cos(lat1),cos(distance/R)-sin(lat1)*sin(p2.latitude)))*180 / PI
  p2.latitude = p2.latitude *180 / PI

  return p2

    def test_telemetry_serializer(self):
        """Tests telemetry serializer."""
        # Set up test data.
        navsat = NavSatFix()
        navsat.latitude = 38.149
        navsat.longitude = -76.432
        navsat.altitude = 30.48
        compass = Float64(90.0)

        json = serializers.TelemetrySerializer.from_msg(navsat, compass)
        altitude_msl = serializers.meters_to_feet(navsat.altitude)

        # Compare.
        self.assertEqual(json["latitude"], navsat.latitude)
        self.assertEqual(json["longitude"], navsat.longitude)
        self.assertEqual(json["altitude_msl"], altitude_msl)
        self.assertEqual(json["uas_heading"], compass.data)

def talker():
	
	print 'in talker'
	pub = rospy.Publisher('GPS', NavSatFix)
	rospy.init_node('GPStalker')
	while not rospy.is_shutdown():
		#Assuming that parse will return these values
		time.sleep(1)
		parse()
		msg = NavSatFix()
		Fix = NavSatStatus()
		Fix.status = GPS.mode
		Fix.service = GPS.numSat
		
		msg.header.stamp = rospy.Time.now()
		msg.status = Fix
		msg.latitude = GPS.lat
		msg.longitude = GPS.lon
		msg.altitude = GPS.alt
		msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		msg.position_covariance_type = 0
		#covariance_type = 0 unknown
		#                = 1 approximated
		#                = 2 diagonal known
		#                = 3 known
		pub.publish(msg)

def get_directions_service(req):
    p = xy2geo(init_pos,req.start)
    orig = "%f,%f"%(p.latitude,p.longitude)
    p = xy2geo(init_pos,req.goal)
    dest = "%f,%f"%(p.latitude,p.longitude)
    print "From ", orig
    print "To ", dest
    print "--------"
    direction_list,status = get_directions(orig,dest)
    #print status
    res = Path()
    for wp in direction_list:
      point = NavSatFix()
      point.latitude = float(wp.start_location[0])
      point.longitude = float(wp.start_location[1])
      pos = geo2xy(init_pos,point)
      res.poses.append(pos)
    return res

 def convert_to_pose(self, x, y):
     if not self.latlong_service:
         rospy.loginfo("Waiting for AddLatLongGoal service...")
         rospy.wait_for_service('/latlong_goal_node/AddLatlongGoal')
         rospy.loginfo("AddLatLongGoal service responded")
         self.latlong_service = rospy.ServiceProxy('/latlong_goal_node/AddLatlongGoal', AddLatlongGoal)
     # create NavSatFix msg
     nav_msg = NavSatFix()
     nav_msg.header.stamp = rospy.get_rostime()
     nav_msg.header.frame_id = 'odom'
     # x is longitude, y is latitude
     nav_msg.longitude = float(x)
     nav_msg.latitude = float(y)
     # convert using /latlong_goal_node/AddLatLongGoal service
     # returns PoseStamped
     response = self.latlong_service(nav_msg)
     rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x,
                   response.goal_xy.pose.position.y))
     return response.goal_xy.pose.position.x, response.goal_xy.pose.position.y

    def parse_gps(self, data):
        '''
        Given raw data from Jaguar GPS socket, parse and return NavSatFix message.
        If bad/incomplete data, return None
        '''
        # use regex to parse
        gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data)
        gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data)

        if gprmc_hit:
            # Get estimated heading (not part of standard ROS navsatfix message)
            gprmc = gprmc_hit.group(0).split(",")
            try:
                heading = float(gprmc[8])
            except:
                heading = float("NaN")
            else:
                while heading < -180.0:
                    heading += 360.0
                while heading > 180.0:
                    heading -= 360.0
            finally:
                self.current_est_heading = heading
        
        if gpgga_hit:
            gpgga = gpgga_hit.group(0).split(",")
            
            nav_msg = NavSatFix()
            # Set header information 
            time = gpgga[1]
            hrs = float(time[0:1])
            mins = float(time[2:3])
            secs = float(time[4:5])
            nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs)
            nav_msg.header.frame_id = "gps"
            # Set GPS Status
            status = NavSatStatus()
            status.status = -1 if int(gpgga[6]) == 0 else 0
            nav_msg.status = status
            # Set longitude and latitude
            lat_str = gpgga[2]
            lon_str = gpgga[4]
            lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0)
            lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0)
            nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs
            nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs
            # Set altitude (Positive is above the WGS 84 ellipsoid)
            try:
                nav_msg.altitude = float(gpgga[9])
            except:
                nav_msg.altitude = float("NaN")
            # Set covariance type to unknown
            nav_msg.position_covariance_type = 0

            return nav_msg
        else:
            return None