def to_msg(self):
h = Header()
h.stamp = self.stamp
h.frame_id = earth_frame_id
msg = NavSatFix()
msg.header = h
msg.latitude = math.degrees(self.geopoint.latitude)
msg.longitude = math.degrees(self.geopoint.longitude)
msg.altitude = self.geopoint.z
return msg
def publish_next_goal(self):
gps_pub = rospy.Publisher('/gps_goal_fix', NavSatFix, queue_size=10)
gps_data = NavSatFix()
gps_data.header.frame_id = '/world'
self.lat = self.lat + 0.00000004000
gps_data.latitude = self.lat
rospy.loginfo('latitude is : ' + str(self.lat))
gps_data.longitude = self.lng
rospy.loginfo('longitude is : ' + str(self.lng))
rospy.loginfo('publishing next goal')
gps_pub.publish(gps_data)
def save_gps_fix_data(bag, kitti, gps_frame_id, topic):
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
navsatfix_msg = NavSatFix()
navsatfix_msg.header.frame_id = gps_frame_id
navsatfix_msg.header.stamp = rospy.Time.from_sec(
float(timestamp.strftime("%s.%f")))
navsatfix_msg.latitude = oxts.packet.lat
navsatfix_msg.longitude = oxts.packet.lon
navsatfix_msg.altitude = oxts.packet.alt
navsatfix_msg.status.service = 1
bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp)
def publish_llh_msg(self, msg, **metadata):
llh_msg = NavSatFix()
llh_msg.latitude = msg.lat
llh_msg.longitude = msg.lon
llh_msg.altitude = msg.height
llh_msg.position_covariance_type = 2
llh_msg.position_covariance = [9, 0, 0, 0, 9, 0, 0, 0, 9]
# Publish ROS messages
self.pub_llh.publish(llh_msg)
self.pub_llh_n_sats.publish(Int32(msg.n_sats))
self.pub_llh_fix_mode.publish(Int32(msg.flags))
def CallGNSS_(self, data): # GNSS data # 1Hz
self.GNSS_ = data
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF_.iterPosCal(self.GNSS_, 'WLSGNSS')
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_.ecef_)
navfix_.header.stamp.secs = self.GNSS_.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.GNSS_Navfix_pub.publish(navfix_) # print 'ecef',self.puGNSSPosCalF_.ecef_,'llh',llh_
def gps_talker():
rospy.loginfo("GPS: Node started.")
pub = rospy.Publisher('gps', NavSatFix, queue_size=10)
rospy.init_node('gps', anonymous=True)
msg = NavSatFix()
seq = 0
lastPub = time.monotonic()
while not rospy.is_shutdown():
gps.update()
currentTime = time.monotonic()
# publish every second
if currentTime - lastPub >= 1.0:
seq += 1
lastPub = currentTime
# fill header
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
# log if gps knows its position (fix)
if prev_has_fix != gps.has_fix:
if gps.has_fix:
rospy.loginfo("GPS: has fix")
else:
rospy.loginfo("GPS: lost fix")
prev_has_fix = gps.has_fix
if not gps.has_fix:
# Try again if no fix
msg.status.status = int(0)
# publish
pub.publish(msg)
continue
else:
msg.status.status = int(1)
msg.status.service = int(0)
# position
msg.latitude = float(gps.latitude)
msg.longitude = float(gps.longitude)
#!!!!!!using altitude as velocity!!!!!
if gps.gps.speed_knots is not None:
msg.altitude = float(gps.speed_knots) * 1.852
# publish
pub.publish(msg)
def parse_goals(self):
waypoints = WaypointsArray()
w_list = []
for location in self.goals['Goals']:
current = NavSatFix()
current.latitude = float(location['lat'])
current.longitude = float(location['long'])
current.altitude = float(location['elev'])
w_list.append(current)
waypoints.waypoints = w_list
self.waypoint_pub.publish(waypoints)
def publish_next_goal(self,i):
self.i = self.i+1
gps_pub = rospy.Publisher('/gps_goal_fix',NavSatFix,queue_size=10)
gps_data = NavSatFix()
gps_data.header.frame_id='/world'
rospy.logerr('number is '+str(self.i))
gps_data.latitude=self.px[self.i]
rospy.loginfo('latitude is : '+str(self.px[self.i]))
gps_data.longitude=self.py[self.i]
rospy.loginfo('longitude is : '+str(self.py[self.i]))
rospy.loginfo('publishing next goal')
gps_pub.publish(gps_data)
def publish_gps(gps_pub, imu_data, log=False):
gps = NavSatFix()
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
gps.latitude = imu_data.lat
gps.longitude = imu_data.lon
gps.altitude = imu_data.alt
gps_pub.publish(gps)
if log:
rospy.loginfo("gps msg published")
def callgnss_ins_navsat(self,data):
self.uck_bestPos_ = NavSatFix()
self.uck_bestPos_ = data
self.ukf_lat_.append(float(self.uck_bestPos_.latitude))
self.ukf_lon_.append(float(self.uck_bestPos_.longitude))
fold = KML.Folder(KML.Placemark(KML.Point(KML.coordinates(str(self.ukf_lon_[0]) + ',' + str(self.ukf_lat_[0]) + ',0'))))
for i in range(1, len(self.ukf_lon_)):
fold.append(KML.Placemark(
KML.Point(KML.coordinates(str(self.ukf_lon_[i]) + ',' + str(self.ukf_lat_[i]) + ',0'))))
content = etree.tostring(etree.ElementTree(fold), pretty_print=True)
with open('Berkeley_gnss_ins.kml', 'w') as fp:
fp.write(content)
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 callback(data):
# Set Map Origin
# 30.174885, -96.512199
olat = 30.174885
olon = -96.512199
# Odom to lat/lon
xg2, yg2 = gc.xy2ll(data.pose.pose.position.x, data.pose.pose.position.y,
olat, olon)
# build navsat message
fake_gps = NavSatFix()
fake_gps.header.frame_id = rospy.get_param('~frame_id', 'gps')
fake_gps.header.stamp = rospy.Time.now()
fake_gps.status.status = 1
fake_gps.status.service = 1
fake_gps.latitude = xg2
fake_gps.longitude = yg2
fake_gps.altitude = 0
#compass = Float(90.0)
pub = rospy.Publisher('fake_gps', NavSatFix, queue_size=10)
pub.publish(fake_gps)
# Odom to UTM
utmy, utmx, utmzone = gc.LLtoUTM(xg2, yg2)
# build navsat message
fake_UTM = NavSatFix()
fake_UTM.header.frame_id = rospy.get_param('~frame_id', 'utm')
fake_UTM.header.stamp = rospy.Time.now()
fake_UTM.status.status = 1
fake_UTM.status.service = 1
fake_UTM.latitude = utmy
fake_UTM.longitude = utmx
fake_UTM.altitude = 0
#compass = Float(90.0)
pub2 = rospy.Publisher('fake_utm', NavSatFix, queue_size=10)
pub2.publish(fake_UTM)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu_Exclusion', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ',enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
def __init__(self):
self.latitude_deg = 0.0
self.longitude_deg = 0.0
self.msl_height_m = 0.0
self.el_height_m = 0.0
self.horizontal_stdev = 0.0
self.vertical_stdev = 0.0
self.valid_llh_flag = 0
self.fix_valid_flags = 0
self.fix_type = 0
self.fix_num_space_vehicles = 0
self.hw_sensor_state = 0
self.hw_antenna_state = 0
self.hw_antenna_power = 0
self.hw_valid_hw_flags = 0
self.publish_data = False
self._HVMsgData = NavSatFix()
self._HVMsgPub = rospy.Publisher('/segway/feedback/gps/fix_3d',
NavSatFix,
queue_size=10)
self._HVMsgData.header.frame_id = '/segway/gps_frame'
self._HVMsgData.status.service = NavSatStatus.SERVICE_GPS
self._HMsgData = NavSatFix()
self._HMsgPub = rospy.Publisher('/segway/feedback/gps/fix_2d',
NavSatFix,
queue_size=10)
self._HMsgData.header.frame_id = '/segway/gps_frame'
self._HMsgData.status.service = NavSatStatus.SERVICE_GPS
self._seq = 0
self.LAT_LON_FIX_VALID = 0x0001
self.ELLIPSOID_HEIGHT_FIX_VALID = 0x0002
self.MSL_HEIGHT_FIX_VALID = 0x0004
self.HORIZONTAL_ACCURACY_VALID = 0x0008
self.VERTICAL_ACCURACY_VALID = 0x0010
def CreateBag(matfile, bagname, frame_id, navsat_topic="/fix"):
'''Creates the actual bag file by successively adding images'''
bag = rosbag.Bag(bagname, 'w', compression=rosbag.Compression.BZ2, chunk_threshold=32 * 1024 * 1024)
data = loadmat(matfile)['rnv']
rnv = {}
for i, col in enumerate(data.dtype.names):
rnv[col] = data.item()[i]
try:
ref = None
x, y = [], []
for i, t in enumerate(tqdm(rnv['t'])):
# print("Adding %s" % image_name)
stamp = rospy.Time.from_sec(t)
nav_msg = NavSatFix()
nav_msg.header.stamp = stamp
nav_msg.header.frame_id = "map"
nav_msg.header.seq = i
nav_msg.latitude = rnv['lat'][i][0]
nav_msg.longitude = rnv['lon'][i][0]
nav_msg.altitude = -rnv['depth'][i][0]
nav_msg.position_covariance_type = nav_msg.COVARIANCE_TYPE_UNKNOWN
transform_msg = TransformStamped()
transform_msg.header = copy(nav_msg.header)
utm = fromLatLong(nav_msg.latitude, nav_msg.longitude, nav_msg.altitude)
if ref is None:
ref = utm.toPoint()
x.append(utm.toPoint().x - ref.x)
y.append(utm.toPoint().y - ref.y)
dx = x[-1] - sum(x[-min(20, len(x)):]) / min(20, len(x))
dy = y[-1] - sum(y[-min(20, len(y)):]) / min(20, len(y))
transform_msg.transform.translation.x = x[-1]
transform_msg.transform.translation.y = y[-1]
transform_msg.transform.translation.z = nav_msg.altitude
transform_msg.transform.rotation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0,
math.atan2(dy, dx)))
transform_msg.child_frame_id = frame_id
tf_msg = tfMessage(transforms=[transform_msg])
odometry_msg = Odometry()
odometry_msg.header = copy(transform_msg.header)
odometry_msg.child_frame_id = frame_id
odometry_msg.pose.pose.position = transform_msg.transform.translation
odometry_msg.pose.pose.orientation = transform_msg.transform.rotation
bag.write(navsat_topic, nav_msg, stamp)
bag.write("/tf", tf_msg, stamp)
bag.write("/transform", transform_msg, stamp)
bag.write("/odom", odometry_msg, stamp)
finally:
bag.close()
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN 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 pubs[i].publish(navsat) rate.sleep()
def talker():
pub = rospy.Publisher('/RTK', Point, queue_size=10)
rospy.init_node('RUN', anonymous=True)
rate = rospy.Rate(100) # 10hz
count = 0
#print("ok")
global sequence_number
global z
while not rospy.is_shutdown():
while True:
line = str(str(s.readline())[0:])
# print("angle is ok")
if line.startswith('$GNGGA'):
#print(line)
a = line.split(',')
RTK = (a[6])
#print(RTK)
record_item = []
sequence_number += 1
msg = pynmea2.parse(line)
lat = msg.latitude
lgi = msg.longitude
navsat = NavSatFix()
navsat.status = 0
navsat.header.seq = 1
navsat.header.stamp = rospy.Time.now()
navsat.position_covariance_type = 2
# navsat.STATUS = 1
navsat.header.frame_id = 'base_link'
navsat.latitude = lat
navsat.longitude = lgi
navsat.altitude = txt()
print(navsat)
utm = Proj(proj='utm', zone=48, ellps='WGS84')
x, y = utm(lgi, lat)
utm_point = Point()
#msg = geom_msg.Pose()
utm_point.x = x
utm_point.y = y
#utm_point.angular.z = txt()
# br = tf.TransformBroadcaster()
# br.sendTransform((x, y, 0),
# tf.transformations.quaternion_from_euler(0, 0, txt()),rospy.Time.now(),"base_link","odom")
#rospy.loginfo(utm_point)
pub.publish(utm_point)
def scooby():
gps1 = NavSatFix()
pub = rospy.Publisher("/rtk_new", NavSatFix, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10)
for i in range(1, 2000):
gps1.latitude = 42
gps1.longitude = -71
gps1.altitude = i
pub.publish(gps1)
print "Publishing data:"
print i
rate.sleep()
def sensor_data_updated(self, carla_gnss_event):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_event: carla gnss event object
:type carla_gnss_event: carla.GnssEvent
"""
navsatfix_msg = NavSatFix()
navsatfix_msg.header = self.get_msg_header(use_parent_frame=False)
navsatfix_msg.latitude = carla_gnss_event.latitude
navsatfix_msg.longitude = carla_gnss_event.longitude
navsatfix_msg.altitude = carla_gnss_event.altitude
self.publish_ros_message(self.topic_name() + "/fix", navsatfix_msg)
def __init__(self):
State.__init__(self, outcomes=['success'], input_keys=['waypoints', 'cur_waypoint_in'])
self.odom_topic = rospy.get_param('~odom_topic', default='odom')
self.gps_topic = rospy.get_param('~gps_topic', default='fix')
self.pose = PoseWithCovarianceStamped()
self.last_gps = NavSatFix()
self.odom_sub = rospy.Subscriber(self.odom_topic, PoseWithCovarianceStamped, self._odom_cb)
self.gps_sub = rospy.Subscriber(self.gps_topic, NavSatFix, self._gps_cb)
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
def sensor_data_updated(self, carla_gnss_measurement):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_measurement: carla gnss measurement object
:type carla_gnss_measurement: carla.GnssMeasurement
"""
navsatfix_msg = NavSatFix()
navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp)
navsatfix_msg.latitude = carla_gnss_measurement.latitude
navsatfix_msg.longitude = carla_gnss_measurement.longitude
navsatfix_msg.altitude = carla_gnss_measurement.altitude
self.gnss_publisher.publish(navsatfix_msg)
def talker(self):
rospy.Subscriber("fix", NavSatFix, self.callback)
pub = rospy.Publisher("navsat/fix", NavSatFix, queue_size=50)
rospy.init_node('navsat_republisher', anonymous=True)
r = rospy.Rate(10)
msg = NavSatFix()
while not rospy.is_shutdown():
msg = self.callback_msg
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame_id
pub.publish(msg)
r.sleep()
def testAutoOriginFromNavSatFix(self):
rospy.init_node('test_initialize_origin')
nsf_pub = rospy.Publisher('gps', NavSatFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
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"
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
nsf_pub.publish(nsf_msg)
r.sleep()
def main():
pub = rospy.Publisher("goal_coords", NavSatFix, queue_size=10)
rospy.init_node("goal_pub", anonymous=False)
r = rospy.Rate(1)
while not rospy.is_shutdown():
msg = NavSatFix()
msg.header.frame_id = "gps_frame"
msg.longitude = 100
msg.latitude = 100
msg.position_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]
pub.publish(msg)
r.sleep()
print(msg)
def publish_gnss(self, sensor_id, data):
"""
Function to publish gnss data
"""
msg = NavSatFix()
msg.header = self.get_header()
msg.header.frame_id = 'gps'
msg.latitude = data[0]
msg.longitude = data[1]
msg.altitude = data[2]
msg.status.status = NavSatStatus.STATUS_SBAS_FIX
msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO
self.publisher_map[sensor_id].publish(msg)
def responseCallback(self, msg):
#############################################################################
gps_msg = NavSatFix()
gps_msg.header = msg.header
gps_msg.status = msg.status
gps_msg.latitude = msg.latitude
gps_msg.longitude = msg.longitude
gps_msg.altitude = msg.altitude
gps_msg.position_covariance_type = msg.position_covariance_type
gps_msg.position_covariance[0] = 1.8
gps_msg.position_covariance[4] = 1.8
gps_msg.position_covariance[8] = 5
self.gpsPub.publish(gps_msg)
def publishCurLatLon(self):
msg = NavSatFix()
msg.header.stamp = rospy.Time(self.curTime)
msg.latitude = self.curLat
msg.longitude = self.curLon
# very small error for now
msg.position_covariance = [0] * 9
msg.position_covariance[0] = 5**2
msg.position_covariance[3] = 5**2
rospy.loginfo("Publishing position %f, %f at time %f", self.curLat,
self.curLon, self.curTime)
self.fixPub.publish(msg)
def callback_gps_position(self, gps_input=NavSatFix()):
lon = [48.1374, 48.1555]
lat = [11.575, 11.579]
lon.append(gps_input.longitude)
lat.append(gps_input.latitude)
alt = gps_input.altitude
print "Longitude: ", gps_input.longitude, "\tLatitude: ", gps_input.latitude, "\tAltitude: ", gps_input.altitude
gmap = gmplot.GoogleMapPlotter(48.1374, 11.575, 13)
gmap.plot(lon, lat, 'cornflowerblue', edge_width=10)
gmap.scatter(lon, lat, 'm', marker=True)
gmap.draw("mymap.html")
def _publish_gnss(self, carla_gnss_measurement):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_measurement: carla gnss measurement object
:type carla_gnss_measurement: carla.GnssMeasurement
"""
navsatfix_msg = NavSatFix()
set_timestamp(navsatfix_msg.header, carla_gnss_measurement.timestamp)
navsatfix_msg.header.frame_id = self._gnss_frame
navsatfix_msg.latitude = carla_gnss_measurement.latitude
navsatfix_msg.longitude = carla_gnss_measurement.longitude
navsatfix_msg.altitude = carla_gnss_measurement.altitude
self.publish("fix", navsatfix_msg)
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)
