def callcptBestPos_(self, data):
self.bestPos_ = BESTPOS()
self.bestPos_ = data
self.GNSSArr1 = GNSS_Raw_Array()
pos_index = 0
if (self.endFlag == 1):
if ((self.bestPos_.header.gps_week_seconds) in self.GNSS_time):
print 'GNSS_time.index(self.bestPos_.header.gps_week_seconds)', self.bestPos_.header.gps_week_seconds
print self.GNSS_time.index(
self.bestPos_.header.gps_week_seconds)
print '-------------------------------------------'
pos_index = self.GNSS_time.index(
self.bestPos_.header.gps_week_seconds)
totalSv_ = self.total_sv[pos_index]
self.GNSS_1 = GNSS_Raw()
for index_ in range(int(totalSv_)):
index_ = index_ + pos_index
print 'index_', index_
self.GNSS_1.GNSS_time = float(
self.GNSS_time[index_]
) # GNSS time contained in csv file
self.GNSS_1.total_sv = float(
self.total_sv[index_]
) # total sitellites in one epoch (epoch: each time point)
self.GNSS_1.prn_satellites_index = float(
self.prn_satellites_index[index_])
self.GNSS_1.pseudorange = float(self.pseudorange[index_])
self.GNSS_1.snr = float(
self.snr[index_]
) # signal noise ratio for this satellite
self.GNSS_1.elevation = float(
self.elevation[index_]
) # elevation for this satellite and receiver
self.GNSS_1.azimuth = float(
self.azimuth[index_]) # azimuth for this satellite
self.GNSS_1.err_tropo = float(
self.err_tropo[index_]) # troposphere error in meters
self.GNSS_1.err_iono = float(
self.err_iono[index_]) # ionophere error in meters
self.GNSS_1.sat_clk_err = float(
self.sat_clk_err[index_]
) # satellite clock bias caused error
self.GNSS_1.sat_pos_x = float(
self.sat_pos_x[index_]
) # satellite positioning in x direction
self.GNSS_1.sat_pos_y = float(
self.sat_pos_y[index_]
) # satellite positioning in y direction
self.GNSS_1.sat_pos_z = float(
self.sat_pos_z[index_]
) # satellite positioning in Z direction
if (self.GNSS_1.elevation > 18.0): # mask angle = 18
self.GNSSArr1.GNSS_Raws.append(self.GNSS_1)
self.GNSS_1 = GNSS_Raw()
self.GNSSArr1.header.seq = self.seq
self.GNSSArr1.header.stamp.secs = self.stampSecs
self.GNSSArr1.header.stamp.nsecs = self.stampnSecs
self.GNSSArr1.header.frame_id = 'GNSS_'
self.GNSS_Raw_pub.publish(self.GNSSArr1)
def readCSV(self):
self.csv_GNSS = csv.reader(open(
self.csvPath, 'r')) # read csv context to csv_reader variable
self.GNSSArr = GNSS_Raw_Array()
self.GNSSTim = 0
self.preGNSSTim = 0
self.linNum = 0
self.preTopiTim = time.time()
self.curTopTim = time.time()
for rowCsv in self.csv_GNSS:
self.preTopiTim = float(round(time.time() * 1000))
self.GNSS_ = GNSS_Raw()
self.rosNavSaFix = NavSatFix()
self.GNSSTim = rowCsv[0]
self.GNSS_.GNSS_time = float(
rowCsv[0]) # GNSS time contained in csv file
self.GNSS_.total_sv = float(
rowCsv[1]
) # total sitellites in one epoch (epoch: each time point)
self.GNSS_.prn_satellites_index = float(
rowCsv[2]
) # satellite index in this epoch (epoch: each time point)
self.GNSS_.pseudorange = float(
rowCsv[3]) - float(rowCsv[7]) - float(rowCsv[8]) + float(
rowCsv[9]) # pseudorange measurement
self.GNSS_.snr = float(
rowCsv[4]) # signal noise ratio for this satellite
self.GNSS_.elevation = float(
rowCsv[5]) # elevation for this satellite and receiver
self.GNSS_.azimuth = float(rowCsv[6]) # azimuth for this satellite
self.GNSS_.err_tropo = float(
rowCsv[7]) # troposphere error in meters
self.GNSS_.err_iono = float(rowCsv[8]) # ionophere error in meters
self.GNSS_.sat_clk_err = float(
rowCsv[9]) # satellite clock bias caused error
self.GNSS_.sat_pos_x = float(
rowCsv[10]) # satellite positioning in x direction
self.GNSS_.sat_pos_y = float(
rowCsv[11]) # satellite positioning in y direction
self.GNSS_.sat_pos_z = float(
rowCsv[12]) # satellite positioning in Z direction
self.GNSS_.visable = 0 # satellite visability juded by NLOS exclusion
if ((self.preGNSSTim == self.GNSSTim) or (self.linNum == 0)):
if (self.GNSS_.elevation > self.eleThres_) and (
self.GNSS_.snr >
self.snrThres_) and (self.GNSS_.prn_satellites_index
not in self.lowEleSatLis_):
self.GNSSArr.GNSS_Raws.append(self.GNSS_)
else:
self.GNSSArr.header.frame_id = 'GNSS_'
self.GNSS_Raw_pub.publish(self.GNSSArr)
# GNSSPosCal_ = puGNSSPosCal.GNSSPosCal()
# GNSSPosCal_.iterPosCal(self.GNSSArr,'LSGNSS')
# print 'llh',GNSSPosCal_.llh_,len(GNSSPosCal_.azimuth),GNSSPosCal_.azimuth
self.GNSSArr_ = self.GNSSArr # keep for outside
del self.GNSSArr.GNSS_Raws[:]
if (self.GNSS_.elevation > self.eleThres_) and (
self.GNSS_.snr >
self.snrThres_) and (self.GNSS_.prn_satellites_index
not in self.lowEleSatLis_):
self.GNSSArr.GNSS_Raws.append(self.GNSS_)
self.curTopTim = float(round(time.time() * 1000))
time.sleep(1 / self.topHz -
(self.curTopTim - self.preTopiTim) / 1000.0)
# time.sleep(1 / self.topHz )
print 'rostopic time bias / ms ------------ ', self.curTopTim - self.preTopiTim
self.preGNSSTim = self.GNSSTim
self.linNum = self.linNum + 1